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) points, poses, names = [], [], [] for position, orientation in args.viewpoints: position = airsim.Vector3r(*position) orientation = airsim.Quaternionr(*orientation) # xyzw points.append(position) poses.append(airsim.Pose(position, orientation)) names.append( ff.to_xyz_str(position) + "\n" + ff.to_xyzw_str(orientation)) plot_linestrips = True # else plot_transforms curr_press_keys = set() key_combination = {keyboard.Key.space, keyboard.Key.shift} def on_press(key): nonlocal plot_linestrips, curr_press_keys, key_combination if key in key_combination: curr_press_keys.add(key) if curr_press_keys == {keyboard.Key.space}: if (plot_linestrips := not plot_linestrips): client.simPlotLineStrip(points, thickness=3, duration=10) else: client.simPlotTransforms(poses, thickness=3, scale=40, duration=10)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: # Reset the drone client.reset() client.enableApiControl(True) client.armDisarm(True) # Draw the ROI outline (erasing previous plots) client.simFlushPersistentMarkers() if SHOW_PLOTS: client.simPlotLineStrip(points=args.roi.corners(repeat_first=True), is_persistent=True) # Get the first position the drone will fly to initial_pose = client.simGetVehiclePose() closest_corner = args.roi.closest_corner(initial_pose.position) if args.verbose: ff.print_pose(initial_pose, airsim.to_eularian_angles) ff.log_info(f"Closest corner {ff.to_xyz_str(closest_corner)}") start_pos = Vector3r( *ff.to_xyz_tuple(closest_corner if args.corner else args.roi.center)) # NOTE AirSim uses NED coordinates, so negative Z values are "up" actually if (z := args.z_offset): start_pos.z_val -= z # start higher up, to avoid crashing with objects
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: initial_pose = client.simGetVehiclePose() initial_state = client.getMultirotorState() if args.verbose: ff.print_pose(initial_pose, airsim.to_eularian_angles) if initial_state.landed_state == airsim.LandedState.Landed: print("[ff] Taking off") client.takeoffAsync(timeout_sec=8).join() # else: # client.hoverAsync().join() # airsim.LandedState.Flying path = [airsim.Vector3r(*position) for position, _orientation in args.viewpoints] future = client.moveOnPathAsync( path, velocity=2, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=-1.5), # FIXME ) _take_pictures_loop(client) future.join() client.reset() print("[ff] Drone reset")
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) if args.flush: client.simFlushPersistentMarkers() # NOTE we have to use (-y, -x, -z), and not (x, -y, -z), because these # files were exported from Unreal Engine as FBX with Force Front XAxis BUILDING = False STATUE = True def convert_position(p, t, s): if BUILDING or STATUE: if t is not None: p += t if s is not None: p *= s x, y, z = map(float, p) return Vector3r(-y, -x, -z) else: return convert_uavmvs_to_airsim_position(p, t, s) points = [ # convert_uavmvs_to_airsim_position(_, translation=args.offset, scaling=args.scale) convert_position(_, args.offset, args.scale) for _ in args.points[::args.every_k] ] client.simPlotPoints(points, Rgba.Blue, POINT_CLOUD_POINT_SIZE, is_persistent=True) if args.trajectory is not None: camera_poses, camera_positions = [], [] for i, camera in enumerate(args.trajectory): if not camera.spline_interpolated: # XXX pose = convert_uavmvs_to_airsim_pose(camera=camera, translation=args.offset, scaling=args.scale) # print(f"=== {i} ===") # print(camera) # print(pose) camera_poses.append(pose) camera_positions.append(pose.position) # client.simPlotLineStrip(camera_positions, Rgba.Black, TRAJECTORY_THICKNESS, is_persistent=True) # client.simPlotPoints(camera_positions, Rgba.White, CAMERA_POSE_SIZE, is_persistent=True) client.simPlotTransforms(camera_poses, 10 * CAMERA_POSE_SIZE, is_persistent=True) if args.edit: enter_edit_mode(client, points)
def fly(client: MultirotorClient, args: argparse.Namespace) -> None: reset(client) rect = cast(Rect, args.rect) initial_pose = cast(Pose, client.simGetVehiclePose()) closest_corner = rect.closest_corner(initial_pose.position) if args.verbose: ff.print_pose(initial_pose, airsim.to_eularian_angles) ff.log_info(f"Closest corner {ff.to_xyz_str(closest_corner)}") start_pos = Vector3r( *ff.to_xyz_tuple(closest_corner)) # NOTE make sure we copy it by value start_pos.z_val += args.z_offset start_pos.y_val += args.y_offset start_pos.x_val += args.x_offset plot = Plotter(client) plot.flush_persistent() plot.points([start_pos], Rgba.Red) if args.teleport: ff.log(f"Teleporting to {ff.to_xyz_str(start_pos)}...") new_pose = Pose(start_pos, initial_pose.orientation) Controller.teleport(client, to=new_pose, ignore_collision=True) else: ff.log(f"Flying to {ff.to_xyz_str(start_pos)}...") client.moveToPositionAsync(*ff.to_xyz_tuple(start_pos), velocity=5).join() ff.log("Capturing grid...") time.sleep(2) z = Vector3r(0, 0, args.altitude_shift) path = [ z + Vector3r(*ff.to_xyz_tuple(corner)) for corner in rect.zigzag(lanes=args.zigzags) ] if AUGMENT_PATHS: path = Controller.augment_path(path, AUGMENT_PATHS_MAX_DIST) plot.points(path, Rgba.White) plot.lines(path, Rgba.Green) try: if RECORD: client.startRecording() Controller.fly_path(client, path) except Exception as e: raise e finally: if RECORD: client.stopRecording()
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None: initial_pose = client.simGetVehiclePose() initial_state = client.getMultirotorState() if args.verbose: ff.print_pose(initial_pose, to_eularian_angles=airsim.to_eularian_angles) if initial_state.landed_state == airsim.LandedState.Landed: print("[ff] Taking off") client.takeoffAsync(timeout_sec=8).join() else: client.hoverAsync().join() # airsim.LandedState.Flying print("[ff] Flying viewpoints...") print("[ff] Press [space] to take pictures (or any other key to stop)" ) # TODO future = _move_by_path(client, args) img_count = 0 while True: if msvcrt.kbhit(): if msvcrt.getch() != b" ": break # https://stackoverflow.com/a/13207813 response, *_ = client.simGetImages([ airsim.ImageRequest( ff.CameraName.front_center, airsim.ImageType.Scene, False, True, # compressed PNG image ) ]) img_count += 1 print(f" {img_count} pictures taken", end="\r") airsim.write_file( os.path.join(args.output_folder, f"out_{img_count}.png"), response.image_data_uint8, ) # TODO save poses to .log file print("camera_position:", response.camera_position) # Vector3r print("camera_orientation:", response.camera_orientation) # Quaternionr print() print(f"[ff] Waiting for drone to finish path...", end=" ", flush=True) future.join() print("done") time.sleep(4) client.reset() print("[ff] Drone reset")
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}"')