Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def RedGateSpawner(client, num_gates, noise_amp):
gate_poses=[]
for idx in range(num_gates):
noise = (np.random.random()-0.5)*noise_amp
pose = Pose(Vector3r(10+idx*9, noise*5.0, 10.0), Quaternionr(0.0, 0.0, 0.707, 0.707))
client.simSpawnObject("gate_"+str(idx), "RedGate16x16", pose, 1.5)
gate_poses.append(pose)
return gate_poses
def rosPose2airsimPose(ros_pose):
t = Vector3r(ros_pose.position.x, ros_pose.position.y, ros_pose.position.z)
q = Quaternionr(ros_pose.orientation.x, ros_pose.orientation.y, ros_pose.orientation.z, ros_pose.orientation.w)
return Pose(t, q)
point.header.stamp = rospy.Time(10) # set an arbitrary time instance
t = tf.TransformerROS()
t.setTransform(m)
# finally we get the gate coord in world coordinates
point_converted = t.transformPoint('world_frame', point)
t_o_g = Vector3r(point_converted.point.x, point_converted.point.y, point_converted.point.z) # now as a world coord vector
# create rotation of gate
# find vector t_b_g in world coordinates
t_b_g = t_o_g - p_o_b.position
phi_quad_ref = np.arctan2(t_b_g.y_val, t_b_g.x_val)
phi_gate = phi_quad_ref + phi_rel
q = transformations.quaternion_from_euler(phi_gate, 0, 0, axes='rzyx')
p_o_g = Pose(t_o_g, Quaternionr(q[0], q[1], q[2], q[3]))
return p_o_g
# takeoff_orientation = airsim.Vector3r(1, 0, 0)
# client.plot_tf([takeoff_pose], duration=20.0, vehicle_name=drone_name)
# client.moveOnSplineAsync([airsim.Vector3r(0, 0, -3)], vel_max=15.0, acc_max=5.0, vehicle_name=drone_name, viz_traj=True).join()
client.moveOnSplineVelConstraintsAsync([takeoff_position], [takeoff_orientation], vel_max=vel_max, acc_max=acc_max, vehicle_name=drone_name, viz_traj=True).join()
# client.moveOnSplineVelConstraintsAsync([airsim.Vector3r(1, 0, 8)], [airsim.Vector3r(1, 0, 0)], vel_max=vel_max, acc_max=acc_max, vehicle_name=drone_name, viz_traj=True)
time.sleep(1.0)
img_res = 64
# path_weights = '/home/rb/data/model_outputs/reg_6/reg_model_10.ckpt'
path_weights = '/home/rb/data/model_outputs/cmvae_9/cmvae_model_20.ckpt'
gate_regressor = gate_regressor.GateRegressor(regressor_type='cmvae', path_weights=path_weights)
while True:
img_batch_1, cam_pos, cam_orientation = process_image(client, img_res)
p_o_b = airsim.types.Pose(cam_pos, cam_orientation)
gate_pose = gate_regressor.predict_gate_pose(img_batch_1, p_o_b)
print(gate_pose.position)
print('Before move spline')
move_drone(client, gate_pose)
print('Done move spline')
time.sleep(1.0)
def randomQuadPose(x_range, y_range, z_range, yaw_range, pitch_range, roll_range):
x = randomSample(x_range)
y = randomSample(y_range)
z = randomSample(z_range)
yaw = randomSample(yaw_range)
pitch = randomSample(pitch_range)
roll = randomSample(roll_range)
q = transformations.quaternion_from_euler(yaw, pitch, roll, axes='rzyx')
t_o_b = Vector3r(x,y,z)
q_o_b = Quaternionr(q[0], q[1], q[2], q[3])
return Pose(t_o_b, q_o_b), yaw
def MoveCheckeredGates(client):
gate_names_sorted = sorted(client.simListSceneObjects("Gate.*"))
# gate_names_sorted_bad is ['Gate0', 'Gate10_21', 'Gate11_23', 'Gate1_3', 'Gate2_5', 'Gate3_7', 'Gate4_9', 'Gate5_11', 'Gate6_13', 'Gate7_15', 'Gate8_17', 'Gate9_19']
# number after underscore is unreal garbage
pose_far = Pose(Vector3r(0,0,1), Quaternionr())
for gate in gate_names_sorted:
client.simSetObjectPose(gate, pose_far)
# time.sleep(0.05)
t_o_g = Vector3r(point_converted.point.x, point_converted.point.y, point_converted.point.z) # now as a world coord vector
# t_o_g = p_o_b.position + t_b_g
# create rotation of gate
# find vector t_b_g in world coordinates
t_b_g = t_o_g - p_o_b.position
phi_quad_ref = np.arctan2(t_b_g.y_val, t_b_g.x_val)
eps = 0 # np.pi/15.0
phi_rel_range = [-np.pi + eps, 0 - eps]
phi_rel = randomSample(phi_rel_range)
phi_gate = phi_quad_ref + phi_rel
# HACK: to make the red side appear, add 180 to gate pose to be spawned
# TODO: remove this hack later!!!!!!!!!!!!!!!!!!!!!!!!!!!
q = transformations.quaternion_from_euler(phi_gate + np.pi, 0, 0, axes='rzyx')
# q = transformations.quaternion_from_euler(phi_gate, 0, 0, axes='rzyx')
p_o_g = Pose(t_o_g, Quaternionr(q[0], q[1], q[2], q[3]))
return p_o_g, r, theta, psi, phi_rel