def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.verbose:
print(
f"[ff] HomeGeoPoint: {Vec3.from_GeoPoint(client.getHomeGeoPoint())}\n"
)
print(
f"[ff] VehiclePose.position: {Vec3.from_Vector3r(client.simGetVehiclePose().position)}\n"
)
initial_state = client.getMultirotorState()
if initial_state.landed_state == airsim.LandedState.Landed:
print(f"[ff] Taking off")
client.takeoffAsync(timeout_sec=8).join()
else:
client.hoverAsync().join() # airsim.LandedState.Flying
#__move_on_path(client, args.flight_path, args.flight_velocity)
#__move_on_box(client, z=-20, side=20, velocity=args.flight_velocity)
if not args.use_viewpoints:
future = client.moveOnPathAsync([
airsim.Vector3r(coord.x, coord.y, coord.z)
for coord in args.flight_path
], args.flight_velocity)
else:
import viewpoints
future = client.moveOnPathAsync(
[airsim.Vector3r(*position) for position in viewpoints.Positions],
args.flight_velocity)
print(f"[ff] Press [space] to take pictures")
ch, img_count, img_responses = msvcrt.getch(), 0, []
while ch == b' ':
img_responses.extend(
client.simGetImages([
airsim.ImageRequest(
ff.CameraName.bottom_center,
airsim.ImageType.Scene,
False,
True # compressed PNG image
)
]))
img_count += 1
print(f" {img_count} pictures taken", end="\r")
ch = msvcrt.getch()
print()
print(f"[ff] Waiting for drone to finish path...", end=" ", flush=True)
future.join()
print(f"done.")
for i, response in enumerate(img_responses):
airsim.write_file(os.path.join(args.output_folder, f"out_{i}.png"),
response.image_data_uint8)
time.sleep(4)
print(f"[ff] Drone reset")
client.reset()
def _move_by_vehicle_poses(client: airsim.MultirotorClient,
args: argparse.Namespace) -> None:
raise Warning("simSetVehiclePose() is meant for ComputerVision mode")
# NOTE check https://github.com/microsoft/AirSim/pull/2324
for position, orientation in args.viewpoints:
pose = airsim.Pose(airsim.Vector3r(*position),
airsim.Quaternionr(*orientation))
client.simSetVehiclePose(pose, ignore_collision=True)
client.hoverAsync().join()
time.sleep(2)
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 _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 __move_on_box(client: airsim.MultirotorClient, z: float, side: float,
velocity: float) -> None:
# NOTE airsim.DrivetrainType.MaxDegreeOfFreedom allows controlling the drone yaw independently
# from the direction it is flying, this way we make it always point to the inside of the box
duration = side / velocity
vx_vy_yaw = [(velocity, 0, 90), (0, velocity, 180), (-velocity, 0, 270),
(0, -velocity, 0)]
print(f"[ff] Moving on box...", end=" ", flush=True)
for vx, vy, yaw in vx_vy_yaw:
client.simPrintLogMessage(
f"moving by velocity vx={vx}, vy={vy}, yaw={yaw}")
client.moveByVelocityZAsync(vx, vy, z, duration,
airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(False, yaw)).join()
time.sleep(4)
client.hoverAsync().join()
client.landAsync().join()
print(f"done.")
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 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}"')
