def _fly_path(client: airsim.MultirotorClient, path: List[Vector3r], velocity: float, timeout_sec: float) -> None: waypoint_count = len(path) assert waypoint_count >= 2 # FIXME handle corner cases first_point, *middle_points, final_point = [ Vec3.from_Vector3r(waypoint) for waypoint in path ] client.moveToPositionAsync(*first_point, velocity, timeout_sec).join() for next_pos in middle_points: # https://github.com/Microsoft/AirSim/issues/1677 # https://github.com/Microsoft/AirSim/issues/2974 future = client.moveToPositionAsync(*next_pos, velocity, timeout_sec) curr_pos = Vec3.from_Vector3r(client.simGetVehiclePose().position) # "spin lock" until we are close to the next point while not Vec3.all_close(curr_pos, next_pos, eps=CONFIRMATION_DISTANCE): curr_pos = Vec3.from_Vector3r(client.simGetVehiclePose().position) time.sleep(WAIT_TIME) # FIXME slow down instead of calling `.join()`... otherwise we're just # using the high level controller in fact (and remove `.sleep()`) future.join() # wait for AirSim's API to also recognize we've arrived time.sleep(0.5) # FIXME stopping for some time minimizes overshooting client.moveToPositionAsync(*final_point, velocity, timeout_sec).join()
def _move_by_positions(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: for position, orientation in args.viewpoints: _pitch, _roll, yaw = airsim.to_eularian_angles( airsim.Quaternionr(*orientation)) client.moveToPositionAsync( *position, args.flight_velocity, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=yaw), ).join() client.hoverAsync().join() time.sleep(2)
def _fly_path2(client: airsim.MultirotorClient, path: List[Vector3r], velocity: float, timeout_sec: float) -> None: # NOTE testing changes to _fly_path (the same FIXMEs apply) assert len(path) >= 2 first_point, *middle_points, final_point = [ Vec3.from_Vector3r(waypoint) for waypoint in path ] client.moveToPositionAsync(*first_point, velocity, timeout_sec).join() for next_pos, next_next_pos in zip(middle_points, middle_points[1:] + [final_point]): future = client.moveToPositionAsync(*next_pos, velocity, timeout_sec) curr_pos = Vec3.from_Vector3r(client.simGetVehiclePose().position) while not Vec3.all_close(curr_pos, next_pos, eps=CONFIRMATION_DISTANCE): curr_pos = Vec3.from_Vector3r(client.simGetVehiclePose().position) time.sleep(WAIT_TIME) # TODO Interpolate between "no slowdown" before the next waypoint # and a "full stop" if the turning angle is too high: # # No stop ... Full stop # # curr next next_next curr next # o -------> o -------> o o -------> o # \___/ ... \__ | # θ = 180 θ = 90 | # v # o # next_next curr_to_next = next_pos - curr_pos next_to_next_next = next_next_pos - next_pos theta = Vec3.angle_between(curr_to_next, next_to_next_next) ff.log_debug(f"θ = {theta}") if theta > 1.0: client.cancelLastTask() future = client.moveToPositionAsync(*next_pos, 1, timeout_sec) future.join() time.sleep(0.2) client.moveToPositionAsync(*final_point, velocity, timeout_sec).join()
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: if args.z is not None and args.z > 0: print("Warning: AirSim uses NED coordinates, meaning +z is down") print(" (to fly upwards use negative z values)\n") state = client.getMultirotorState() start_pos = { 'NED': get_NED_coords(client, state), 'UE4': get_UE4_coords(client, state) } # obs.: UE4's PlayerStart coordinates correspond to ~(0, 0, 0) in AirSim's NED system coords_format = "({:.2f} {:.2f} {:.2f})" print("Starting position (NED): {}".format(coords_format).format( *start_pos['NED'])) if args.verbose: print("Starting position (UE4): {}".format(coords_format).format( *start_pos['UE4'])) if args.relative: if args.z is None: args.z = 0 xyz = tuple( sum(axis) for axis in zip(start_pos['NED'], (args.x, args.y, args.z))) else: if args.z is None: args.z = start_pos['NED'][2] # keep the same altitude xyz = (args.x, args.y, args.z) if state.landed_state == airsim.LandedState.Landed: print(f"\nlanded_state = {state.landed_state} (Landed)") print("[ff] Taking off... ", end="", flush=True) client.takeoffAsync().join() print("done.") else: # NOTE .exe environments seem to always return Landed print(f"\nlanded_state = {state.landed_state} (Flying)") client.hoverAsync().join() if args.teleport: print("[ff] Teleporting to {}... ".format(coords_format).format(*xyz), end="", flush=True) pose = airsim.Pose() pose.position = airsim.Vector3r(*xyz) client.simSetVehiclePose(pose, ignore_collision=True) time.sleep(4) # wait a few seconds after teleporting else: print("[ff] Moving to {}... ".format(coords_format).format(*xyz), end="", flush=True) client.moveToPositionAsync(*xyz, args.velocity).join() print("done.") print(f"[ff] Hovering for {args.wait_sec} seconds... ", end="", flush=True) client.hoverAsync().join() time.sleep(args.wait_sec) print("done.\n") state = client.getMultirotorState() print("[ff] Ending position (NED): {}".format(coords_format).format( *get_NED_coords(client, state))) if args.verbose: print("[ff] Ending position (UE4): {}".format(coords_format).format( *get_UE4_coords(client, state)))
def teleport(client: airsim.MultirotorClient, to: Pose, ignore_collision: bool = True) -> None: # HACK see https://github.com/Microsoft/AirSim/issues/1618#issuecomment-689152817 client.simSetVehiclePose(to, ignore_collision) client.moveToPositionAsync(*ff.to_xyz_tuple(to.position), velocity=1)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: initial_pose = client.simGetVehiclePose() if args.verbose: ff.print_pose(initial_pose, airsim.to_eularian_angles) ff.log(client.simGetCameraInfo(camera_name=CAPTURE_CAMERA)) if args.flush or (args.capture_dir and not args.debug): client.simFlushPersistentMarkers() camera_poses = [] for camera in args.trajectory: position = convert_uavmvs_to_airsim_position( camera.position, translation=args.offset, scaling=args.scale ) orientation = quaternion_orientation_from_eye_to_look_at(position, LOOK_AT_TARGET) camera_poses.append(Pose(position, orientation)) if args.debug: camera_positions = [pose.position for pose in camera_poses] client.simPlotPoints(camera_positions, Rgba.Blue, is_persistent=True) client.simPlotLineStrip(camera_positions, Rgba.Cyan, thickness=2.5, is_persistent=True) airsim_record = [] def do_stuff_at_uavmvs_viewpoint(i, pose): nonlocal client, camera_poses, airsim_record log_string = f"({i}/{len(camera_poses)})" p, q = pose.position, pose.orientation if args.debug: log_string += f" position = {to_xyz_str(p)}" client.simPlotTransforms([pose], scale=100, is_persistent=True) # client.simPlotArrows([p], [LOOK_AT_TARGET], Rgba.White, thickness=3.0, duration=10) elif args.capture_dir: path = f"{args.prefix}pose{args.suffix}_{i:0{len(str(len(camera_poses)))}}.png" path = os.path.join(args.capture_dir, path) airsim.write_png(path, AirSimImage.get_mono(client, CAPTURE_CAMERA)) log_string += f' saved image to "{path}"' record_line = AirSimRecord.make_line_string(p, q, time_stamp=str(i), image_file=path) airsim_record.append(record_line) ff.log(log_string) if IS_CV_MODE: for i, camera_pose in enumerate(camera_poses): client.simSetVehiclePose(camera_pose, ignore_collision=True) do_stuff_at_uavmvs_viewpoint(i, camera_pose) time.sleep(CV_SLEEP_SEC) else: client.moveToZAsync(z=-10, velocity=VELOCITY).join() # XXX avoid colliding on take off client.hoverAsync().join() mean_position_error = 0.0 for i, camera_pose in enumerate(camera_poses): client.moveToPositionAsync( *to_xyz_tuple(camera_pose.position), velocity=VELOCITY, drivetrain=DrivetrainType.MaxDegreeOfFreedom, yaw_mode=YawMode(is_rate=False, yaw_or_rate=YAW_N), ).join() with pose_at_simulation_pause(client) as real_pose: # NOTE when we pre-compute the viewpoint's camera orientation, we use the # expected drone position, which (should be close, but) is not the actual # drone position. Hence, we could experiment with using fake orientation: # quaternion_orientation_from_eye_to_look_at(real_pose.position, LOOK_AT_TARGET) fake_pose = Pose(real_pose.position, camera_pose.orientation) client.simSetVehiclePose(fake_pose, ignore_collision=True) client.simContinueForFrames(1) # NOTE ensures pose change do_stuff_at_uavmvs_viewpoint(i, fake_pose) client.simSetVehiclePose(real_pose, ignore_collision=True) position_error = real_pose.position.distance_to(camera_pose.position) mean_position_error += position_error ff.log_debug(f"{position_error = }") mean_position_error /= len(camera_poses) ff.log_debug(f"{mean_position_error = }") if airsim_record: print_to_file = args.record_path is not None file = open(args.record_path, "w") if print_to_file else None print(AirSimRecord.make_header_string(), file=(file if print_to_file else sys.stdout)) for line in airsim_record: print(line, file=(file if print_to_file else sys.stdout)) if print_to_file: file.close() ff.log_info(f'Saved AirSim record to "{args.record_path}"')
class Drone(Thread): def __init__(self, telemetry: Telemetry): super(Drone, self).__init__() self.daemon = True self._exit = False self.telemetry = telemetry self.client: Optional[MultirotorClient] = None self.connect() self.controller = Controller(self.client, self.telemetry) self.collision_type = CollisionType.NONE def run(self): while not self._exit: try: self._process() except BufferError: pass except KeyboardInterrupt: self.shutdown() def _process(self): self.update_telemetry() if self.is_collision(): self.process_collision() elif self.telemetry.collision_mode: self.stop_collision() else: self.controller.check_progress() time.sleep(0.1) def shutdown(self): self._exit = True self.client.enableApiControl(False) self.client.reset() def connect(self): self.client = MultirotorClient() self.client.confirmConnection() self.client.enableApiControl(True) self.client.getBarometerData() # region TELEMETRY def update_telemetry(self): multirotor_state = self.client.getMultirotorState() self.telemetry.landed_state = multirotor_state.landed_state self.telemetry.ned_position = multirotor_state.kinematics_estimated.position self.telemetry.linear_velocity = multirotor_state.kinematics_estimated.linear_velocity self.telemetry.gps_position = self.client.getGpsData().gnss.geo_point self.telemetry.gps_home = self.client.getHomeGeoPoint() self.telemetry.quaternion = self.client.simGetVehiclePose().orientation self.get_front_camera_image() self.get_bottom_camera_image() def get_bottom_camera_image(self): self.client.simSetCameraOrientation('0', Quaternionr(0.0, -0.7, 0.0, 0.5)) bottom_camera_data = self.client.simGetImages([ airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, pixels_as_float=True) ]) self.telemetry.terrain_collision.process_image(bottom_camera_data[0]) def get_front_camera_image(self): self.client.simSetCameraOrientation('0', Quaternionr(0.0, 0.0, 0.0, 1.0)) front_camera_data = self.client.simGetImages([ airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, pixels_as_float=True) ]) self.telemetry.wall_collision.process_image(front_camera_data[0]) # endregion # region COLLISION def is_collision(self): collision = (self.telemetry.wall_collision.collision or self.telemetry.terrain_collision.collision) return collision def process_collision(self): wall_collision = self.telemetry.wall_collision.collision terrain_collision = self.telemetry.terrain_collision.collision if wall_collision: if terrain_collision: self.wall_collision() self.collision_type = CollisionType.BOTH else: self.wall_collision() self.collision_type = CollisionType.WALL else: if terrain_collision: self.terrain_collision() self.collision_type = CollisionType.TERRAIN else: self.collision_type = CollisionType.NONE def wall_collision(self): if (self.collision_type != CollisionType.WALL and self.collision_type != CollisionType.BOTH): self.telemetry.collision_mode = True self.client.moveToZAsync(-500, settings.COLLISION_SPEED) def terrain_collision(self): if self.collision_type != CollisionType.TERRAIN: if self.telemetry.ned_position.z_val < -8: self.telemetry.collision_mode = True target = self.telemetry.target_position actual = self.telemetry.ned_position self.client.moveToPositionAsync(target.x_val, target.y_val, actual.z_val - 1, settings.COLLISION_SPEED) def stop_collision(self): self.controller.send_position() self.telemetry.collision_mode = False