Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
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)
def polarTranslation(r, theta, psi):
# follow math convention for polar coordinates
# r: radius
# theta: azimuth (horizontal)
# psi: vertical
x = r * np.cos(theta) * np.sin(psi)
y = r * np.sin(theta) * np.sin(psi)
z = r * np.cos(psi)
return Vector3r(x, y, z)
def polarTranslation(r, theta, psi):
# follow math convention for polar coordinates
# r: radius
# theta: azimuth (horizontal)
# psi: vertical
x = r * np.cos(theta) * np.sin(psi)
y = r * np.sin(theta) * np.sin(psi)
z = r * np.cos(psi)
return Vector3r(x, y, z)
m.transform.translation.y = p_o_b.position.y_val
m.transform.translation.z = p_o_b.position.z_val
point = geometry_msgs.msg.PointStamped()
point.point.x = t_b_g.x_val
point.point.y = t_b_g.y_val
point.point.z = t_b_g.z_val
point.header.frame_id = 'base_frame'
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
# Initialize the IMAGE variables -- We Take in Front Center, Right, Left
self.images_rgb = None
self.images_rgba = None
self.image_mask_rgb = np.array([ [0+3*i,1+3*i,2+3*i] for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1)
self.image_mask_rgba = np.array([ [0+4*i,1+4*i,2+4*i] for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1)
self.image_mask_FC_FR_FL = image_mask_FC_FR_FL
# Connect to the AirSim simulator and begin:
print('Initializing Car Client')
self.client = client.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
print('Initialization DONE!')
print("Setting Camera Views")
orien = Vector3r(0, 0, 0)
self.client.simSetCameraOrientation("0", orien) #radians
orien = Vector3r(0, .12, -np.pi/9)
self.client.simSetCameraOrientation("1", orien)
orien = Vector3r(0, .12, np.pi/9)
self.client.simSetCameraOrientation("2", orien)
# Reset Collion Flags
print("Setting Camera Views DONE!")
# Set up GUI Video Feeder
self.gui_data = {'obs': None, 'state': None, 'meta': None}
self.vehicle_name = vehicle_name
num_video_feeds = np.sum(np.array(self.image_mask_FC_FR_FL, dtype = np.int)) * IMG_STEP
GUIConn, self.simEnvDataConn = multiprocessing.Pipe()
self.app = AirSimGUI.CarGUI(GUIConn, vehicle_names = [vehicle_name],
num_video_feeds = num_video_feeds, isRGB = isRGB, isNormal = isNormal)
rgb = "%d %d %d" % color
projectionMatrix = np.array([[-0.501202762, 0.000000000, 0.000000000, 0.000000000],
[0.000000000, -0.501202762, 0.000000000, 0.000000000],
[0.000000000, 0.000000000, 10.00000000, 100.00000000],
[0.000000000, 0.000000000, -10.0000000, 0.000000000]])
# Connect to the AirSim simulator and begin:
print('Initializing Car Client')
client = airsim.CarClient()
client.confirmConnection()
client.enableApiControl(False)
print('Initialization DONE!')
print("Setting Camera Views")
orien = Vector3r(0, 0, 0)
client.simSetCameraOrientation("3", orien) #radians
orien = Vector3r(0, 0, -np.pi/2)
client.simSetCameraOrientation("1", orien)
orien = Vector3r(0, 0, np.pi/2)
client.simSetCameraOrientation("2", orien)
orien = Vector3r(0, 0, np.pi/2)
# Reset Collion Flags
print("Setting Camera Views DONE!")
while True:
print("Making a Point Cloud!")
images = client.simGetImages([airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, False, False), # Front Center
airsim.ImageRequest("1", airsim.ImageType.DepthPlanner, False, False), # Front Right
airsim.ImageRequest("2", airsim.ImageType.DepthPlanner, False, False)]) # Front Left
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
# Initialize the IMAGE variables -- We Take in Front Center, Right, Left
self.images_rgb = None
self.images_rgba = None
self.image_mask_rgb = np.array([ [0+3*i,1+3*i,2+3*i] for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1)
self.image_mask_rgba = np.array([ [0+4*i,1+4*i,2+4*i] for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1)
self.image_mask_FC_FR_FL = image_mask_FC_FR_FL
# Connect to the AirSim simulator and begin:
print('Initializing Car Client')
self.client = client.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
print('Initialization DONE!')
print("Setting Camera Views")
orien = Vector3r(0, 0, 0)
self.client.simSetCameraOrientation(0, orien) #radians
orien = Vector3r(0, .12, -np.pi/9)
self.client.simSetCameraOrientation(1, orien)
orien = Vector3r(0, .12, np.pi/9)
self.client.simSetCameraOrientation(2, orien)
# Reset Collion Flags
print("Setting Camera Views DONE!")
# Set up GUI Video Feeder
self.gui_data = {'obs': None, 'state': None, 'meta': None}
self.vehicle_name = vehicle_name
num_video_feeds = np.sum(np.array(self.image_mask_FC_FR_FL, dtype = np.int)) * IMG_STEP
GUIConn, self.simEnvDataConn = multiprocessing.Pipe()
self.app = AirSimGUI.CarGUI(GUIConn, vehicle_names = [vehicle_name],
num_video_feeds = num_video_feeds, isRGB = isRGB, isNormal = isNormal)
self.max_altitude = max_altitude
self.min_altitude = min_altitude
# Connect to the AirSim simulator and begin:
print('Initializing Client')
self.client = client.MultirotorClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.client.armDisarm(True)
print('Initialization Complete!')
print("Setting Camera Views")
orien = Vector3r(0, 0, 0)
self.client.simSetCameraOrientation(0, orien) #radians
orien = Vector3r(0, .12, -np.pi/9)
self.client.simSetCameraOrientation(1, orien)
orien = Vector3r(0, .12, np.pi/9)
self.client.simSetCameraOrientation(2, orien)
# Reset Collion Flags
print("Setting Camera Views DONE!")
# Set up GUI Video Feeder
self.gui_data = {'obs': None, 'state': None, 'meta': None}
self.vehicle_name = vehicle_name
num_video_feeds = np.sum(np.array(self.image_mask_FC_FR_FL, dtype = np.int)) * IMG_STEP
GUIConn, self.simEnvDataConn = multiprocessing.Pipe()
self.app = AirSimGUI.QuadcopterGUI(GUIConn, vehicle_names = [vehicle_name],
num_video_feeds = num_video_feeds, isRGB = isRGB, isNormal = isNormal)
# Timing Operations Initialize
self.time_to_do_action = 0
self.time_to_grab_images = 0
self.time_to_grab_states = 0