def main(args: argparse.Namespace) -> None:
if args.verbose:
ff.print_airsim_path(airsim.__path__)
preflight(args) # setup
client = connect(ff.SimMode.ComputerVision)
try:
fly(client, args) # do stuff
except KeyboardInterrupt:
client.reset() # avoid UE4 'fatal error' when exiting with Ctrl+C
finally:
ff.log("Done")
def preflight(args: argparse.Namespace) -> None:
assert os.path.isfile(
args.ply_path), f"Invalid file path: '{args.ply_path}'"
# Parse the point cloud into a pandas dataframe and extract its points
ply_df = read_ply(args.ply_path)
if args.verbose:
ff.log(ply_df)
args.points = ply_df["points"][["x", "y", "z"]].to_numpy()
n_of_points, _ = args.points.shape
print(f"The point cloud has {n_of_points} points.")
# NOTE avoid plotting all points for large clouds, since Unreal can't handle it
k = 0 if args.every_k is None else args.every_k
while not k:
try:
k = int(
input_or_exit(
"Do you want to plot one every how many points? "))
except KeyboardInterrupt:
exit()
except:
k = 0
print("\nPlease input a valid integer. ", end="")
continue
answer = input_or_exit(
f"This will plot a total of {n_of_points // k} points"
f" (i.e. one every {k}).\nDo you want to continue? [Y/n] ")
k = 0 if answer.lower().strip() in ["n", "no"] else k
args.every_k = k
# Check if a uavmvs trajectory was passed in and parse it into points
if args.trajectory_path is not None:
_, ext = os.path.splitext(args.trajectory_path)
assert os.path.isfile(args.trajectory_path
), f"Invalid file path: '{args.trajectory_path}'"
assert ext in parse_uavmvs.keys(
), f"Invalid trajectory extension: '{args.trajectory_path}'"
args.trajectory = parse_uavmvs[ext](args.trajectory_path)
if args.verbose:
ff.log(f"The trajectory has {len(args.trajectory)} camera poses")
else:
args.trajectory = None
if args.env_name is not None:
# the --launch option was passed
ff.launch_env(*ff.LaunchEnvArgs(args))
ff.input_or_exit("\nPress [enter] to connect to AirSim ")
def main(args: argparse.Namespace) -> None:
if args.verbose:
ff.print_airsim_path(airsim.__path__)
assert args.env_name is not None, "missing --launch option"
ff.launch_env(*ff.LaunchEnvArgs(args))
if not args.skip_connect:
ff.input_or_exit("\nPress [enter] to connect to AirSim ")
client = connect(ff.SimMode.ComputerVision)
time.sleep(1)
client.reset()
ff.log("Done")
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)
def main(args: argparse.Namespace) -> None:
if args.verbose:
ff.print_airsim_path(airsim.__path__)
preflight(args) # setup
client = connect(SIM_MODE)
try:
fly(client, args) # do stuff
except KeyboardInterrupt:
client.reset() # avoid UE4 'fatal error' when exiting with Ctrl+C
finally:
ff.log("Done\n")
ff.log_info(f"Used scale = {args.scale} and offset = {args.offset}")
ff.log_info(f"Used VELOCITY = {VELOCITY}")
ff.log_info(f"Used SIM_MODE = {SIM_MODE}")
ff.log_info(f"Used LOOK_AT_TARGET = {to_xyz_str(LOOK_AT_TARGET)}")
ff.log_info(f"Used CAPTURE_CAMERA = {CAPTURE_CAMERA}")
def preflight(args: argparse.Namespace) -> None:
path = args.trajectory_path
_, ext = os.path.splitext(path)
assert os.path.isfile(path), path
assert ext in parse_uavmvs.keys(), path
args.trajectory = parse_uavmvs[ext](path)
if args.verbose:
ff.log(f"The trajectory has {len(args.trajectory)} camera poses")
if args.capture_dir:
args.capture_dir = os.path.abspath(args.capture_dir)
assert os.path.isdir(args.capture_dir), args.capture_dir
if args.env_name is not None:
# the --launch option was passed
ff.launch_env(*ff.LaunchEnvArgs(args))
ff.input_or_exit("\nPress [enter] to connect to AirSim ")
def enter_edit_mode(client: airsim.MultirotorClient,
points: List[Vector3r]) -> None:
mode = EditMode.TRANSLATING
step = 1.0
total = {
EditMode.TRANSLATING:
Vector3r(0, 0, 0),
# EditMode.ROTATING: Vector3r(0, 0, 0),
EditMode.SCALING:
Vector3r(1, 1, 1),
}
try:
while True:
key = ord(airsim.wait_key())
if key == 224: # arrow keys
arrow_key = ArrowKey(ord(airsim.wait_key()))
client.simFlushPersistentMarkers()
if mode == EditMode.TRANSLATING:
delta = {
ArrowKey.Up: Vector3r(step, 0, 0),
ArrowKey.Down: Vector3r(-step, 0, 0),
ArrowKey.Left: Vector3r(0, -step, 0),
ArrowKey.Right: Vector3r(0, step, 0),
}[arrow_key]
points = [point + delta for point in points]
total[EditMode.TRANSLATING] += delta
elif mode == EditMode.SCALING:
factor = {
ArrowKey.Up: 1 + step * 0.1,
ArrowKey.Down: 1 - step * 0.1,
ArrowKey.Left: 1 - step * 0.1,
ArrowKey.Right: 1 + step * 0.1,
}[arrow_key]
points = [point * factor for point in points]
total[EditMode.SCALING] *= factor
else:
assert False # TODO handle other edit modes for rotating and scaling
client.simPlotPoints(points,
Rgba.Blue,
POINT_CLOUD_POINT_SIZE,
is_persistent=True)
elif key == 27: # esc
ff.log("TRANSLATING:", total[EditMode.TRANSLATING])
ff.log("SCALING:", total[EditMode.SCALING])
return
else:
if key == ord(b"["):
step /= 2.0
elif key == ord(b"]"):
step *= 2.0
elif key == ord(b"\t"):
mode = EditMode.next(mode)
ff.log(f"{mode=} {step=}")
except KeyboardInterrupt:
return
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()
*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
if (y := args.y_offset): start_pos.y_val += y
if (x := args.x_offset): start_pos.x_val += x
# Plot a point at the start position and go to it
if SHOW_PLOTS:
client.simPlotPoints([start_pos],
color_rgba=Rgba.White,
is_persistent=True)
if args.teleport:
ff.log(f"Teleporting to {ff.to_xyz_str(start_pos)}...")
# NOTE using `nanQuaternionr` for the orientation should work for `simSetVehiclePose`
# not to change it (which is called by `Controller.teleport`)... but it doesn't
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,
timeout_sec=12).join()
# Fly over the region of interest now that we reached the starting position
ff.log("Flying over zone...")
time.sleep(2)
fly_zone(client, args.roi, altitude_shift=args.z_shift)
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}"')
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
# TODO verify we are in "ComputerVision" mode
# if (sim_mode := ff.curr_sim_mode()) != ff.SimMode.ComputerVision:
# assert False, f"Please change the SimMode from '{sim_mode}' to 'ComputerVision'"
if args.roi is not None:
if args.verbose:
ff.log_info(f"Loading ROI from '{args.roi}'\n")
with open(args.roi, "r") as f:
zone = Rect.from_dump(f.read())
else:
zone = Rect(Vector3r(), Vector3r(0, 10, 0), Vector3r(10, 0, 0))
# repeat the first coordinate to close the line strip
client.simPlotLineStrip(points=zone.corners(repeat_first=True), is_persistent=True)
# available keys (i.e. not used by AirSim)
# ,----------------------------------------------------------------------------------------.
# | ` | | | | 4 | 5 | 6 | 7 | 8 | 9 | | -* | = | |
# |----------------------------------------------------------------------------------------+
# | Tab | | | | | | Y | U | I* | O | P | [ | ] | |
# |----------------------------------------------------------------------------------------+
# | CpsLck | | | | | G | H | J | K* | L | | ' | Enter |
# |----------------------------------------------------------------------------------------+
# | Shift | Z | X | C | V | | N | | , | . | | Shift |
# `----------------------------------------------------------------------------------------'
ff.log("Use [z], [x], [c], [v] to move the region of interest (ROI)")
ff.log("Press [lshift] to swap editing modes (translating, scaling)")
ff.log("Press [rshift] to save the current ROI to json")
edit_mode = EditMode.TRANSLATING
# define keyboard callbacks
swap_mode_key = keyboard.Key.shift_l
save_rect_key = keyboard.Key.shift_r
def on_press(key):
nonlocal edit_mode, swap_mode_key, zone
client.simPrintLogMessage("ROI coordinates: ", message_param=str(zone))
if key == swap_mode_key:
edit_mode = EditMode.next(edit_mode, skip_rotation=True)
client.simPrintLogMessage("Current edit mode: ", message_param=edit_mode.name)
elif key == save_rect_key:
filename = save_zone(args.outputdir, zone)
client.simPrintLogMessage(f"Saved ROI coordinates to '{filename}'")
if args.verbose:
ff.log_info(f"Saved ROI coordinates to '{filename}'")
elif edit_zone(key, edit_mode, zone):
client.simFlushPersistentMarkers()
client.simPlotLineStrip(points=zone.corners(repeat_first=True), is_persistent=True)
def on_release(key):
if key == keyboard.Key.esc:
return False # stop the listener
# collect events until released
with keyboard.Listener(on_press=on_press, on_release=on_release) as listener:
ff.log("Press [esc] to quit")
listener.join()
if args.clear:
client.simFlushPersistentMarkers()
