def reset(self): self.client.reset() self.client.enableApiControl(True) self.client.armDisarm(True) # Move the TargetActor to initial position self.client.simSetObjectPose("TargetActor", self.initialPose, True) # Change initial position self._move_target("TargetActor") # Spawn the drone at random position around frontal position offset = airsim.Vector3r(np.random.uniform(-2, 2), np.random.uniform(0, 1), np.random.uniform(-0.5, 0.3)) ori = self.client.simGetObjectPose("TargetActor").orientation while np.isnan(ori.w_val): #print("FAIL") ori = self.client.simGetObjectPose("TargetActor").orientation offset = self._transform_to_frame(offset, ori) dronePos = self.frontalPos + offset self.client.simSetVehiclePose( airsim.Pose(dronePos, airsim.to_quaternion(0, 0, 0)), True) self.client.moveByVelocityZAsync(0, 0, dronePos.z_val, self.duration).join() time.sleep(0.05) # Look at target direction self._look_at_target() self.client.simSetCameraOrientation("bottom_center", airsim.to_quaternion(1.40, 0, 0)) # Reset variables self.start_time = time.time() return self._observe()
def _take_action(self, action): # Baselines stuff action = np.clip(action, [ -self.maxSpeed, -self.maxSpeed, -self.maxSpeed, -self.maxAngle, -self.maxAngle ], [ +self.maxSpeed, +self.maxSpeed, +self.maxSpeed, +self.maxAngle, +self.maxAngle ]) # Drone # action[0] -> forwards/backwards # action[1] -> left/right # action[2] -> up/down v = airsim.Vector3r(action[0], action[1], action[2]) # Get the drone's orientation #ori = self.client.simGetGroundTruthKinematics().orientation ori = self.client.simGetCameraInfo("front_center").pose.orientation v = self._transform_to_frame(v, ori) self.client.moveByVelocityAsync( v.x_val, v.y_val, v.z_val - 0.09, self.duration, airsim.DrivetrainType.MaxDegreeOfFreedom, airsim.YawMode()).join() z = self.client.simGetGroundTruthKinematics().position.z_val self.client.moveByVelocityZAsync(0, 0, z, self.duration).join() # Camera # action[3] -> up/down # action[4] -> left/right qRot = airsim.to_quaternion(action[3], 0, 0) self.qCam = self.qCam * qRot qRot = airsim.to_quaternion(0, 0, action[4]) self.qCam = qRot * self.qCam self.client.simSetCameraOrientation("front_center", self.qCam)
def _take_action(self, action): # Baselines stuff #action = np.clip(action, [-self.maxSpeed,-self.maxSpeed,-self.maxSpeed,-self.maxAngle,-self.maxAngle], [+self.maxSpeed,+self.maxSpeed,+self.maxSpeed,+self.maxAngle,+self.maxAngle]) # Baselines stuff action = np.float64(action) #print(action) # Drone # action[0] -> forwards/backwards # action[1] -> left/right # action[2] -> up/down self.client.moveByVelocityAsync( action[0], action[1], action[2] / 2, self.duration, airsim.DrivetrainType.MaxDegreeOfFreedom, airsim.YawMode()).join() z = self.client.simGetGroundTruthKinematics().position.z_val self.client.moveByVelocityZAsync(0, 0, z, self.duration).join() # Camera # action[3] -> up/down # action[4] -> left/right qRot = airsim.to_quaternion(action[3], 0, 0) self.qCam = self.qCam * qRot qRot = airsim.to_quaternion(0, action[4], 0) self.qCam = qRot * self.qCam self.client.simSetCameraOrientation("bottom_center", self.qCam)
def _setup_my_cameras(self): """ Helper function to set the left, right, forward, and back cameras up on the vehicle as I've see fit. :returns: nada """ left_cam_orientation = airsim.to_quaternion(pitch=-0.17, yaw=-1.04, roll=0.0) forward_cam_orientation = airsim.to_quaternion(pitch=-0.17, yaw=0.0, roll=0.0) right_cam_orientation = airsim.to_quaternion(pitch=-0.17, yaw=1.04, roll=0.0) backward_cam_orientation = airsim.to_quaternion(pitch=-0.17, yaw=0.0, roll=0.0) # creates a panoram-ish camera set-up self.client.simSetCameraOrientation(self.left_cam_name, left_cam_orientation) self.client.simSetCameraOrientation(self.right_cam_name, right_cam_orientation) self.client.simSetCameraOrientation(self.forward_cam_name, forward_cam_orientation)
def _look_at_target(self): # Get the direction of the target object cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position targetDirection = self.targetPos - cameraPos if targetDirection.get_length() > 0: # The default camera pitch defaultDirection = airsim.Vector3r(0, 0, 1) # Calculate the angle between the two pitchTheta = np.arccos( targetDirection.dot(defaultDirection) / (targetDirection.get_length() * defaultDirection.get_length())) #targetDirection.z_val = 0 # for yaw vector # The default camera yaw defaultDirection = airsim.Vector3r(1, 0, 0) # Calculate the angle between the two yawTheta = np.arccos( targetDirection.dot(defaultDirection) / (targetDirection.get_length() * defaultDirection.get_length())) if targetDirection.y_val < defaultDirection.y_val: yawTheta = -yawTheta else: pitchTheta = 0 yawTheta = 0 # Set camera pitch self.qCam = airsim.to_quaternion(pitchTheta, 0, 0) # Set camera yaw self.qCam = airsim.to_quaternion(0, yawTheta, 0) * self.qCam self.client.simSetCameraOrientation("bottom_center", self.qCam)
def _take_action(self, action): #TO_DO ZOOM """camera_info = self.client.simGetCameraInfo("bottom_center") print("CameraInfo %s: %s" % ("bottom_center", pprint.pformat(camera_info))) drone_info = self.client.simGetGroundTruthKinematics() print("DroneInfo %s: %s" % ("drone", pprint.pformat(drone_info)))""" if action == 0: pass elif action == 1: # Camera up qRot = airsim.to_quaternion(0.0872, 0, 0) self.qCam = self.qCam * qRot elif action == 2: # Camera down qRot = airsim.to_quaternion(-0.0872, 0, 0) self.qCam = self.qCam * qRot elif action == 3: # Camera left qRot = airsim.to_quaternion(0, -0.0872, 0) self.qCam = qRot * self.qCam elif action == 4: # Camera right qRot = airsim.to_quaternion(0, 0.0872, 0) self.qCam = qRot * self.qCam self.client.simSetCameraOrientation("bottom_center", self.qCam)
def add_offset_to_pose(pose, pos_offset=1, yaw_offset=np.pi*2, enable_offset=True): if enable_offset: position = airsim.Vector3r(pos_offset*np.random.rand()+pose[0], pos_offset*np.random.rand()+pose[1], -pose[2]) orientation = airsim.to_quaternion(0, 0, yaw_offset*np.random.rand()+pose[3]) pose_with_offset = airsim.Pose(position, orientation) return pose_with_offset else: position = airsim.Vector3r(pose[0], pose[1], -pose[2]) orientation = airsim.to_quaternion(0, 0, pose[3]) pose_without_offset = airsim.Pose(position, orientation) return pose_without_offset
def main(): global nmap, road_points, path_in_list, path_in_environment # finish = turn.execute_turns(client, nmap, road_points, path_in_list, path_in_environment) if len(sys.argv) == 2: client = airsim.CarClient() client.confirmConnection() print('Connect succcefully!') client.enableApiControl(True) car_controls = airsim.CarControls() client.simSetTimeOfDay(True, "2019-08-19 21:00:00", True, 1, 60, False) client.simSetCameraOrientation( 2, airsim.to_quaternion(0, 0, -math.pi / 4)) client.simSetCameraOrientation(1, airsim.to_quaternion(0, 0, math.pi / 4)) start_point = [13, 13] target_point = [0, 22] nmap, road_points, path_in_list, path_in_environment = turn.init_path_planning( start_point, target_point) if sys.argv[1] == 'test': print('start testing') testNetwork(client, car_controls, list(path_in_environment[-1])) elif sys.argv[1] == 'train': print('start training') trainNetwork(client, car_controls) else: print('unkown input argument, please run this file as follow!') print('python DQN_airsim.py train OR python DQN_airsim.py test') elif len(sys.argv) == 3: client = airsim.CarClient() client.confirmConnection() print('Connect succcefully!') if sys.argv[1] == 'show_map' and sys.argv[2] == 'train': print('display map') show_map.show_map(client, []) elif sys.argv[1] == 'show_map' and sys.argv[2] == 'test': print('display map') show_map.show_map(client, path_in_environment) else: print('unkown input argument, please run this file as follow!') print( 'python DQN_airsim.py show_map train OR python DQN_airsim.py show_map test' ) else: print('please include an argument train or test or show_map')
def reset(self): self.client.reset() self.client.enableApiControl(True) self.client.armDisarm(True) # Spawn the drone at random position self.client.simSetVehiclePose( airsim.Pose( airsim.Vector3r(np.random.randint(0, 16), np.random.randint(-10, 11), np.random.randint(-5, -3)), airsim.to_quaternion(0, 0, 0)), True) time.sleep(0.05) # Get the direction of the target object cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position targetDirection = self.targetPos - cameraPos #targetDirection.z_val = 0 # for yaw vector if targetDirection.get_length() > 0: # The default camera pitch defaultDirection = airsim.Vector3r(0, 0, 1) # Calculate the angle between the two pitchTheta = np.arccos( targetDirection.dot(defaultDirection) / (targetDirection.get_length() * defaultDirection.get_length())) # The default camera yaw defaultDirection = airsim.Vector3r(1, 0, 0) # Calculate the angle between the two yawTheta = np.arccos( targetDirection.dot(defaultDirection) / (targetDirection.get_length() * defaultDirection.get_length())) if targetDirection.y_val < defaultDirection.y_val: yawTheta = -yawTheta else: pitchTheta = 0 yawTheta = 0 # Set camera pitch self.qCam = airsim.to_quaternion(pitchTheta, 0, 0) # Set camera yaw self.qCam = airsim.to_quaternion(0, yawTheta, 0) * self.qCam self.client.simSetCameraOrientation("bottom_center", self.qCam) # Perform trajectory self._run_trajectory() return self._observe()
def take_action(self, action, num_actions, phase): # Set Paramaters fov_v = 45 * np.pi / 180 fov_h = 80 * np.pi / 180 r = 0.4 ignore_collision = False sqrt_num_actions = np.sqrt(num_actions) posit = self.client.simGetVehiclePose() pos = posit.position orientation = posit.orientation quat = (orientation.w_val, orientation.x_val, orientation.y_val, orientation.z_val) eulers = euler_from_quaternion(quat) alpha = eulers[2] theta_ind = int(action[0] / sqrt_num_actions) psi_ind = action[0] % sqrt_num_actions theta = fov_v / sqrt_num_actions * (theta_ind - (sqrt_num_actions - 1) / 2) psi = fov_h / sqrt_num_actions * (psi_ind - (sqrt_num_actions - 1) / 2) # print('Theta: ', theta * 180 / np.pi, end='') # print(' Psi: ', psi * 180 / np.pi) if phase == 'train': noise_theta = (fov_v / sqrt_num_actions) / 6 noise_psi = (fov_h / sqrt_num_actions) / 6 psi = psi + random.uniform(-1, 1) * noise_psi theta = theta + random.uniform(-1, 1) * noise_theta x = pos.x_val + r * np.cos(alpha + psi) y = pos.y_val + r * np.sin(alpha + psi) z = pos.z_val + r * np.sin( theta) # -ve because Unreal has -ve z direction going upwards self.client.simSetVehiclePose(airsim.Pose( airsim.Vector3r(x, y, z), airsim.to_quaternion(0, 0, alpha + psi)), ignore_collison=ignore_collision) elif phase == 'infer': r_infer = 0.5 vx = r_infer * np.cos(alpha + psi) vy = r_infer * np.sin(alpha + psi) vz = r_infer * np.sin(theta) # TODO # Take average of previous velocities and current to smoothen out drone movement. self.client.moveByVelocityAsync( vx=vx, vy=vy, vz=vz, duration=1, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=180 * (alpha + psi) / np.pi))
def get_topview_image(hight=100, start_pos=[0, 0], drone=None, tmp_dir="airsim_drone"): drone.client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(start_pos[0], start_pos[1], -hight), airsim.to_quaternion(0, 0, 0)), True, vehicle_name='Drone1') image_responses = drone.client.simGetImages([ airsim.ImageRequest("bottom_center", airsim.ImageType.DepthPlanner, True)], vehicle_name="Drone1") image_response = image_responses[0] img1d = image_response.image_data_float img2d = np.reshape(img1d, (image_response.height, image_response.width)) filename = os.path.join(tmp_dir, "depth_" + str(hight) + '.png') imageio.imwrite(os.path.normpath(os.path.join(tmp_dir, "depth_" + str(hight) + '.png')), generate_contrast_viz(img2d, hight)) # plt.imshow(img2d) # plt.show() real_image_responses = drone.client.simGetImages([ airsim.ImageRequest("bottom_center", airsim.ImageType.Scene, False, False)], vehicle_name="Drone1") img1d = np.fromstring(real_image_responses[0].image_data_uint8, dtype=np.uint8) img_rgb = img1d.reshape(real_image_responses[0].height, real_image_responses[0].width, 3) # plt.imshow(img_rgb) # plt.show() # airsim.write_png(os.path.normpath(os.path.join(tmp_dir, "real_" + str(hight) + '.png')), img_rgb) imageio.imwrite(os.path.normpath(os.path.join(tmp_dir, "real_" + str(hight) + '.png')), img_rgb) return img_rgb, filename
def reset(client, scene=None): """ Called from the reset method in AirsimEnv. This method resets the AirSim simulation, respawns the UAV etc. :param client: AirsimEnv object :param scene: The AirSim scene :return: None """ client.reset() if scene is not None: time.sleep(0.2) pose = client.simGetVehiclePose() start_pos = valid_spawn(scene) pose.position.x_val = start_pos[0] pose.position.y_val = start_pos[1] pose.position.z_val = 0 pitch, roll, yaw = airsim.to_eularian_angles(pose.orientation) yaw = np.random.rand() * 2 * np.pi pose.orientation = airsim.to_quaternion(pitch, roll, yaw) client.simSetVehiclePose(pose, True) time.sleep(0.2) client.enableApiControl(True) client.armDisarm(True) hover(client) custom_takeoff(client) hover(client)
def get_start_pose(self, random=True, verbose=True): # 在路上选择一个起始位置和方向 if not random: # 固定选择默认的起始位置 position = np.array([0., 0.]) yaw = 0. else: # 随机选择一个位置 if not hasattr(self, 'roads_without_corners'): self.roads_without_corners = self.get_roads( include_corners=False) # 计算位置 road_index = np.random.choice(len(self.roads_without_corners)) p, q = self.roads_without_corners[road_index] t = np.random.uniform(0.3, 0.7) position = t * p + (1. - t) * q # 计算朝向 if np.isclose(p[0], q[0]): # 与 Y 轴平行 yaws = [0.5 * math.pi, -0.5 * math.pi] elif np.isclose(p[1], q[1]): # 与 X 轴平行 yaws = [0., math.pi] yaw = np.random.choice(yaws) if verbose: print('起始位置 = {}, 方向 = {}'.format(position, yaw)) position = airsim.Vector3r(position[0], position[1], -0.6) orientation = airsim.to_quaternion(pitch=0., roll=0., yaw=yaw) pose = airsim.Pose(position, orientation) return pose
def test_car_rand_pose(vehicle_client, car_client): # get the fixed location to test the orientation randomness starting_points_fixed, _ = get_random_pose() i = 0 while i < 30: # print('setting position') starting_points, starting_direction = get_random_pose() # set car location and orientation vehicle_client.simSetVehiclePose( airsim.Pose(airsim.Vector3r(starting_points[0], starting_points[1], starting_points[2]), airsim.to_quaternion(starting_direction[0], starting_direction[1], starting_direction[2])), True) # test the car orientation # print(starting_direction) # car_client.simSetVehiclePose( # airsim.Pose(airsim.Vector3r(starting_points_fixed[0], starting_points_fixed[1], starting_points_fixed[2]), # airsim.to_quaternion(starting_direction[0], starting_direction[1], # starting_direction[2] + 0.01)), True) # print('wait for momentum die out') car_controls = airsim.CarControls() car_controls.steering = 0 car_controls.throttle = 0 car_controls.brake = 1 car_client.setCarControls(car_controls) time.sleep(4) i += 1
def initial_positions(name, initZ=0, num_agents = 1): name = name+'()' orig_ip, levels, crash_threshold = eval(name) ip_each_drone = int(np.floor(len(orig_ip) / num_agents)) reset_array = {} reset_array_raw = {} level_names = {} for agents in range(num_agents): name_agent = "drone" + str(agents) ind = ip_each_drone * agents player_start_unreal = orig_ip[ind] reset_array[name_agent] = [] reset_array_raw[name_agent] = [] level_names[name_agent] = [] physical_player_start = orig_ip[0] for i in range(ip_each_drone): x1 = (orig_ip[i+ind][0]-player_start_unreal[0])/100 y1 = (orig_ip[i+ind][1]-player_start_unreal[1])/100 x_raw = (orig_ip[i+ind][0]-physical_player_start[0])/100 y_raw = (orig_ip[i+ind][1]-physical_player_start[1])/100 # z1 = 0 z1 = initZ # in case of computervision mode pitch = 0 roll = 0 yaw = orig_ip[i+ind][2]*np.pi/180 pp = airsim.Pose(airsim.Vector3r(x1, y1, z1), airsim.to_quaternion(pitch, roll, yaw)) reset_array[name_agent].append(pp) reset_array_raw[name_agent].append([x_raw, y_raw, z1, yaw*180/np.pi, roll*180/np.pi, pitch*180/np.pi]) level_names[name_agent].append(levels[ind+i]) return reset_array, reset_array_raw, level_names, crash_threshold
def __init__(self): self.client = airsim.MultirotorClient() self.client.confirmConnection() self.client.enableApiControl(True) self.client.armDisarm(True) orientation = airsim.to_quaternion(-np.pi / 6, 0, 0) self.client.simSetCameraOrientation('0', orientation)
def calculate_camera(self, v_identifier): """ Calculate camera heading """ current_pos_state = self.client.getMultirotorState( ).kinematics_estimated current_pos_local = np.asarray([ current_pos_state.position.x_val, current_pos_state.position.y_val, current_pos_state.position.z_val ]) # print("now:"+str(current_pos_local)) camera_angle = self.calculate_camera_to_center(current_pos_local) global g_current_orientation_pitch, g_current_orientation_roll, g_current_orientation_yaw g_current_orientation_pitch = camera_angle[0] g_current_orientation_roll = 0 g_current_orientation_yaw = camera_angle[ 2] - airsim.to_eularian_angles(current_pos_state.orientation)[2] #g_current_orientation_yaw = camera_angle[2] # log_file("camera", # v_identifier+":desired orientation:" + str(g_current_orientation_pitch) + "," # + str(g_current_orientation_yaw)) self.client.simSetCameraOrientation( "front_center", airsim.to_quaternion(g_current_orientation_pitch, g_current_orientation_roll, g_current_orientation_yaw))
def setCamNum(self, num): # self.client.confirmConnection() # self.client.enableApiControl(True, "Camera1") # self.client.armDisarm(True, "Camera1") #print("CamNum Callback:") self.camNum = num.data print("Switching to Camera: {}".format(self.camNum)) if self.camNum is not 7: print("Cam Num is not 7") self.pose.position.x_val = self.camCoords[ self.camNum][0] - self.offset_x self.pose.position.y_val = self.camCoords[ self.camNum][1] - self.offset_y self.pose.position.z_val = self.camCoords[self.camNum][2] #self.pose.position.w_val = self.camCoords[self.camNum][3] # self.pose.orientation. #self.pose.orientation.w_val = self.camCoords[self.camNum][3]; self.pose.orientation = airsim.to_quaternion( 0, 0, self.camCoords[self.camNum][3]) print(self.pose) #print(self.pose); self.client.simSetVehiclePose(self.pose, True, vehicle_name="Camera1").join()
def reset(self): self.cur_step = 0 self.trajectory.clear_memory() cur_pos = self.get_cur_position() if math.isnan(cur_pos[0]) or math.isnan(cur_pos[1]) or math.isnan(cur_pos[2]): self.client.reset() self.client.enableApiControl(True) self.client.armDisarm(True) orientation = airsim.to_quaternion(-np.pi / 6, 0, 0) self.client.simSetCameraOrientation('0', orientation) # set the starting position of the drone to be at 4 meters away from the human rel_pos = self.local_to_world(np.array([0, -2, -4]), 1) position = self.client.simGetObjectPose(self.HUMAN_ID).position position.z_val -= 1.7 / 2 position.x_val += rel_pos[0] position.y_val += rel_pos[1] position.z_val += rel_pos[2] heading = self.client.simGetObjectPose(self.HUMAN_ID).orientation pose = airsim.Pose(position, heading) self.client.simSetVehiclePose(pose, True) # use songxiaocheng's airsim (https://github.com/songxiaocheng/AirSim) self.client.moveToPositionAsync(position.x_val, position.y_val, position.z_val, 1).join() # use official airsim # self.client.takeoffAsync().join() print("===== start position: ", self.v2t(position))
def AirView(W, X, Y, Z, skeleton_recv, dir=1, rate=1): pp = pprint.PrettyPrinter(indent=4) client = airsim.VehicleClient() client.confirmConnection() # airsim.wait_key('Press any key to start the tracking') x_init, y_init, z_init = 0, 0, -1.6 while True: sk = skeleton_recv.recv() # print('AirSimCV received:', sk) if isinstance(sk, list) and len(sk) == 25: FacePos = sk[0] x_shift = -FacePos[2] / 50 y_shift = FacePos[0] / 212 z_shift = FacePos[1] / 256 #print("received HeadPose:", HeadPose[0]) n_w, n_qx, n_qy, n_qz = W.value, X.value, Y.value, Z.value if dir: client.simSetVehiclePose( airsim.Pose( airsim.Vector3r(x_init + x_shift, y_init + rate * y_shift, z_init + rate * z_shift), airsim.Quaternionr(n_qx, n_qy, n_qz, n_w)), True) else: client.simSetVehiclePose( airsim.Pose( airsim.Vector3r(x_init - x_shift, y_init - rate * y_shift, z_init - rate * z_shift), airsim.Quaternionr(n_qx, n_qy, n_qz, n_w)), True) elif sk == "Break": print("Tracking terminating...") client.simSetPose( airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.0, 0, 0)), True) break elif sk == 'Empty': i = 1 client.simSetPose( airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True)
def setCameraAngle(self, camera_angle, drone, cam=0): """ Set camera angle """ pos = self.client.simSetCameraOrientation( cam, airsim.to_quaternion(camera_angle * math.pi / 180, 0, 0), vehicle_name=drone) # radians
def airpub(): ## Start ROS --------------------------------------------------------------- rospy.init_node('geo_mapping', anonymous=False) rate = rospy.Rate(10) ## Publishers -------------------------------------------------------------- # image publishers depth_pub = rospy.Publisher("airsim/depth", Image, queue_size=1) # camera paramters publisher rgb_cam_pub = rospy.Publisher("airsim/camera_info", CameraInfo, queue_size=1) depth_cam_pub = rospy.Publisher("airsim/depth/camera_info", CameraInfo, queue_size=1) # odometry publisher odom_pub = rospy.Publisher("odom", Odometry, queue_size=1) # pose publisher pose_pub = rospy.Publisher("airsim/pose", PoseStamped, queue_size=1) # curent position publisher current_pose_pub = rospy.Publisher("airsim/current_pose", Vector3, queue_size=1) ## Main -------------------------------------------------------------------- # connect to the AirSim simulator client = airsim.CarClient() client.confirmConnection() client.enableApiControl(True) client.simSetCameraOrientation(0, airsim.to_quaternion(0, 0, 0)) # client.simSetCameraOrientation(0, airsim.to_quaternion(-math.pi/2, 0, 0)) while not rospy.is_shutdown(): camera_info_msg = get_camera_params() simPose = get_sim_pose(client) odom_msg = convert_posestamped_to_odom_msg(simPose) rgb_msg, depth_msg = get_image_messages(client) current_pose = get_curr_pose(client) # header message simPose.header.stamp = rospy.Time.now() odom_msg.header.stamp = simPose.header.stamp camera_info_msg.header.stamp = simPose.header.stamp depth_msg.header = camera_info_msg.header # publish message current_pose_pub.publish(current_pose) pose_pub.publish(simPose) publish_tf_msg(simPose) odom_pub.publish(odom_msg) depth_cam_pub.publish(camera_info_msg) depth_pub.publish(depth_msg) # log PoseStamped message rospy.loginfo(simPose) # sleeps until next cycle rate.sleep()
def start(self): client = airsim.VehicleClient('', self.port) client.confirmConnection() print("1111111111111111") client.simSetCameraOrientation( "0", airsim.to_quaternion(0.261799 * 3, 0, 0)) print("22222222222222222") return "complit"
def as_quaternion(self) -> Quaternionr: """ Returns a quaternion representing the rotation angles. Note: AirSim's `Quaternionr` expresses coordinates in WXYZ order. """ return airsim.to_quaternion( pitch=self.pitch, roll=self.roll, yaw=self.yaw, )
def reset(self): self.client.reset() self.client.enableApiControl(True) self.client.armDisarm(True) # Spawn the drone at random position self.velocity = airsim.Vector3r(0,0,0) self.client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(np.random.uniform(-1,0),np.random.uniform(-1,1),np.random.uniform(-3,-1)), airsim.to_quaternion(0, 0, 0)), True) self.client.moveByVelocityAsync(self.velocity.x_val, self.velocity.y_val, self.velocity.z_val, 1, airsim.DrivetrainType.MaxDegreeOfFreedom, airsim.YawMode()) time.sleep(0.05) # Get the direction of the target object cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position targetDirection = self.targetPos - cameraPos if targetDirection.get_length() > 0: # The default camera pitch defaultDirection = airsim.Vector3r(0,0,1) # Calculate the angle between the two pitchTheta = np.arccos(targetDirection.dot(defaultDirection) / (targetDirection.get_length() * defaultDirection.get_length())) #targetDirection.z_val = 0 # for yaw vector # The default camera yaw defaultDirection = airsim.Vector3r(1,0,0) # Calculate the angle between the two yawTheta = np.arccos(targetDirection.dot(defaultDirection) / (targetDirection.get_length() * defaultDirection.get_length())) if targetDirection.y_val < defaultDirection.y_val: yawTheta = -yawTheta else: pitchTheta = 0 yawTheta = 0 # Set camera pitch self.qCam = airsim.to_quaternion(pitchTheta, 0, 0) # Set camera yaw self.qCam = airsim.to_quaternion(0, yawTheta, 0) * self.qCam self.client.simSetCameraOrientation("bottom_center", self.qCam) self.start_time = time.time() #self._move_to_target() return self._observe()
def reset(self): self.client.reset() self.client.enableApiControl(True) self.client.armDisarm(True) # Move the currently active TargetActor to its initial pose self.client.simSetObjectPose("TargetActor" + str(self.activeMesh), self.initialPose[self.activeMesh], True) # Pick a new TargetActor for this episode self.activeMesh = np.random.randint(0, self.numPeople - 4) # Train #self.activeMesh = np.random.randint(self.numPeople-4,self.numPeople) # Test #self.activeMesh = 4 # Change TargetActor pose self._move_target("TargetActor" + str(self.activeMesh)) # Spawn the drone at random position around frontal position offset = airsim.Vector3r(np.random.uniform(-1, 1), np.random.uniform(0, 0.5), np.random.uniform(-0.2, 0.1)) #offset = airsim.Vector3r(0, 0, -0.06) ori = self._safe_simGetObjectPose("TargetActor" + str(self.activeMesh)).orientation offset = self._transform_to_frame(offset, ori) dronePos = self.frontalPos + offset self.client.simSetVehiclePose( airsim.Pose(dronePos, airsim.to_quaternion(0, 0, 0)), True) self.client.moveByVelocityZAsync(0, 0, dronePos.z_val, self.duration).join() time.sleep(0.05) # Look at target direction self._look_at_target() self.client.simSetCameraOrientation("bottom_center", airsim.to_quaternion(1.40, 0, 0)) # Reset variables self.targetReached = 0 self.start_time = time.time() return self._observe()
def __init__(self, control_mode, max_steering, max_throttle, max_brakes): """ Initializes the AISGame object. Args: control_mode (string): The control mode to used with the joystick. """ self.client = airsim.CarClient() self.client.confirmConnection() self.client.enableApiControl(True) # Camera 2 facing left or right? (negative for left) cam_dir = -1 # Change camera direction (in radians) self.client.simSetCameraOrientation( 1, airsim.to_quaternion(0, 0, cam_dir * 0.523599)) # self.client.simSetCameraOrientation(2, # airsim.to_quaternion(0, # 0, # -0.523599)) # Internally represents the vehicle control state self.vehicle_controls = VehicleControl(control_mode, max_steering, max_throttle, max_brakes) self._timer = None self.save_timer = None self._display = None self._main_image = None self._seg_image = None self._is_on_reverse = False self._position = None self.counter = 0 self.color_map = {} self.val_map = {} self.set_segmentation_ids() self.recording = False self.request_stop_recording = False self.record_path = None self.save_counter = 0 self.csv_data = [] self.last_pos = np.zeros(3) pygame.joystick.init() self.joysticks = [ pygame.joystick.Joystick(x) for x in range(pygame.joystick.get_count()) ] for j in self.joysticks: j.init() print("Found %d joysticks" % (len(self.joysticks)))
def airpub(): ## Start ROS --------------------------------------------------------------- rospy.init_node('airsim_img_publisher', anonymous=False) loop_rate = rospy.get_param('~loop_rate', 10) rate = rospy.Rate(loop_rate) ## Publishers -------------------------------------------------------------- # image publishers rgb_pub = rospy.Publisher("airsim/rgb/image_raw", Image, queue_size=1) depth_pub = rospy.Publisher("airsim/depth", Image, queue_size=1) # camera paramters publisher rgb_cam_pub = rospy.Publisher("airsim/camera_info", CameraInfo, queue_size=1) depth_cam_pub = rospy.Publisher("airsim/depth/camera_info", CameraInfo, queue_size=1) # odometry publisher odom_pub = rospy.Publisher("odom", Odometry, queue_size=1) # pose publisher pose_pub = rospy.Publisher("airsim/pose", PoseStamped, queue_size=1) ## Main -------------------------------------------------------------------- # connect to the AirSim simulator client = airsim.MultirotorClient() client.confirmConnection() client.simSetCameraOrientation(0, airsim.to_quaternion(0, 0, 0)) # client.simSetCameraOrientation(0, airsim.to_quaternion(-math.pi/2, 0, 0)) while not rospy.is_shutdown(): camera_info_msg = get_camera_params() sim_pose_msg = get_sim_pose(client) odom_msg = convert_posestamped_to_odom_msg(sim_pose_msg) rgb_msg, depth_msg = get_image_messages(client) # header message sim_pose_msg.header.stamp = rospy.Time.now() odom_msg.header.stamp = sim_pose_msg.header.stamp camera_info_msg.header.stamp = sim_pose_msg.header.stamp rgb_msg.header = camera_info_msg.header depth_msg.header = camera_info_msg.header # publish message pose_pub.publish(sim_pose_msg) publish_tf_msg(sim_pose_msg) odom_pub.publish(odom_msg) rgb_cam_pub.publish(camera_info_msg) depth_cam_pub.publish(camera_info_msg) rgb_pub.publish(rgb_msg) depth_pub.publish(depth_msg) rate.sleep()
def _take_action(self, action): if action == 0: # Do nothing pass elif action == 1: # Tilt drone camera up qRot = airsim.to_quaternion(0.0872, 0, 0) self.qCam = self.qCam * qRot elif action == 2: # Tilt drone camera down qRot = airsim.to_quaternion(-0.0872, 0, 0) self.qCam = self.qCam * qRot elif action == 3: # Pan drone camera left qRot = airsim.to_quaternion(0, -0.0872, 0) self.qCam = qRot * self.qCam elif action == 4: # Pan drone camera right qRot = airsim.to_quaternion(0, 0.0872, 0) self.qCam = qRot * self.qCam self.client.simSetCameraOrientation("bottom_center", self.qCam)
def OrbitAnimal(cx, cy, radius, speed, altitude, camera_angle, animal): """ @param cx: The x position of our orbit starting location @param cy: The x position of our orbit starting location @param radius: The radius of the orbit circle @param speed: The speed the drone should more, it's hard to take photos when flying fast @param altitude: The altidude we want to fly at, dont fly too high! @param camera_angle: The angle of the camera @param animal: The name of the animal, used to prefix the photos """ x = cx - radius y = cy # set camera angle client.simSetCameraOrientation(0, airsim.to_quaternion( camera_angle * math.pi / 180, 0, 0)) # radians # move the drone to the requested location print("moving to position...") client.moveToPositionAsync( x, y, z, 1, 60, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(False, 0)).join() pos = client.getMultirotorState().kinematics_estimated.position dx = x - pos.x_val dy = y - pos.y_val yaw = airsim.to_eularian_angles( client.getMultirotorState().kinematics_estimated.orientation)[2] # keep the drone on target, it's windy out there! print("correcting position and yaw...") while abs(dx) > 1 or abs(dy) > 1 or abs(yaw) > 0.1: client.moveToPositionAsync( x, y, z, 0.25, 60, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(False, 0)).join() pos = client.getMultirotorState().kinematics_estimated.position dx = x - pos.x_val dy = y - pos.y_val yaw = airsim.to_eularian_angles( client.getMultirotorState().kinematics_estimated.orientation)[2] print("yaw is {}".format(yaw)) print("location is off by {},{}".format(dx, dy)) o = airsim.to_eularian_angles( client.getMultirotorState().kinematics_estimated.orientation) print("yaw is {}".format(o[2])) # let's orbit around the animal and take some photos nav = drone_orbit.OrbitNavigator(photo_prefix=animal, radius=radius, altitude=altitude, speed=speed, iterations=1, center=[ cx - pos.x_val, cy - pos.y_val], snapshots=30, image_dir="./drone_images/") nav.start()
# https://github.com/Microsoft/AirSim/blob/master/docs/image_apis.md#computer-vision-mode import setup_path import airsim import pprint import os import time pp = pprint.PrettyPrinter(indent=4) client = airsim.VehicleClient() client.confirmConnection() airsim.wait_key('Press any key to set camera-0 gimble to 15-degree pitch') client.simSetCameraOrientation("0", airsim.to_quaternion(0.261799, 0, 0)); #radians airsim.wait_key('Press any key to get camera parameters') for camera_name in range(5): camera_info = client.simGetCameraInfo(str(camera_name)) print("CameraInfo %d: %s" % (camera_name, pp.pprint(camera_info))) airsim.wait_key('Press any key to get images') for x in range(3): # do few times z = x * -20 - 5 # some random number client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(z, z, z), airsim.to_quaternion(x / 3.0, 0, x / 3.0)), True) responses = client.simGetImages([ airsim.ImageRequest("0", airsim.ImageType.DepthVis), airsim.ImageRequest("1", airsim.ImageType.DepthPerspective, True), airsim.ImageRequest("2", airsim.ImageType.Segmentation),
def moveUAV(client,pos,yaw): client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(pos[0], pos[1], pos[2]), airsim.to_quaternion(0, 0, yaw)), True)
camera_info = client.simGetCameraInfo(str(camera_id)) print("CameraInfo %d: %s" % (camera_id, pp.pprint(camera_info))) airsim.wait_key('Press any key to get images') tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_drone") print ("Saving images to %s" % tmp_dir) try: for n in range(3): os.makedirs(os.path.join(tmp_dir, str(n))) except OSError: if not os.path.isdir(tmp_dir): raise for x in range(50): # do few times #xn = 1 + x*5 # some random number client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(x, 0, -2), airsim.to_quaternion(0, 0, 0)), True) time.sleep(0.1) responses = client.simGetImages([ airsim.ImageRequest("0", airsim.ImageType.Scene), airsim.ImageRequest("1", airsim.ImageType.Scene), airsim.ImageRequest("2", airsim.ImageType.Scene)]) for i, response in enumerate(responses): if response.pixels_as_float: print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_float), pprint.pformat(response.camera_position))) airsim.write_pfm(os.path.normpath(os.path.join(tmp_dir, str(x) + "_" + str(i) + '.pfm')), airsim.get_pfm_array(response)) else: print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_uint8), pprint.pformat(response.camera_position))) airsim.write_file(os.path.normpath(os.path.join(tmp_dir, str(i), str(x) + "_" + str(i) + '.png')), response.image_data_uint8)
import setup_path import airsim import time # connect to the AirSim simulator client = airsim.MultirotorClient() client.confirmConnection() client.enableApiControl(True) client.armDisarm(True) # MultirotorClient.wait_key('Press any key to takeoff') print("Taking off") client.takeoffAsync().join() print("Ready") for i in range(5): client.moveToPositionAsync(float(-50.00), float( 50.26), float( -20.58), float( 3.5)) time.sleep(6) client.simSetCameraOrientation("0", airsim.to_quaternion(0.5, 0.5, 0.1)) client.moveToPositionAsync(float(50.00), float( -50.26), float( -10.58), float( 3.5)) time.sleep(6) client.simSetCameraOrientation("0", airsim.to_quaternion(-0.5, -0.5, -0.1))
client.reset() airsim.wait_key('Press any key to set skin age to 1') client.simCharSetSkinAgeing(1) airsim.wait_key('Press any key to set skin color to 0.9') client.simCharSetSkinDarkness(0.9) #airsim.wait_key('Press any key to set face expression') #client.simCharSetFaceExpression("BlendShapeNode_Smile", 1); airsim.wait_key('Press any key to set bone pose') client.reset() jaw_pose = airsim.Pose() jaw_pose.position = airsim.Vector3r(0.002, 0.001, -0.003) jaw_pose.orientation = airsim.to_quaternion(0, 0, -.15) client.simCharSetBonePose( "Jaw", jaw_pose); airsim.wait_key('Press any key to set preset') client.reset() for x in range(0, 10, 3): client.simCharSetFacePreset("FACS_0" + str(x), 5); time.sleep(1) airsim.wait_key('Press any key to set multiple presets') presets = {"Phoneme_l":0.5, "Phoneme_ae": 1, "Phoneme_ooo":0.0} client.simCharSetFacePresets(presets) airsim.wait_key('Press any key to turn head around') client.reset() for pitch in range(-5, 5, 5):