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: 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: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.flush:
client.simFlushPersistentMarkers()
if not args.transformation:
poses = [
Pose(record.position, record.orientation)
for record in args.recording.values()
]
positions = [record.position for record in args.recording.values()]
else:
matrix = np.loadtxt(
args.transformation) # load the 4x4 transformation matrix
print(matrix)
poses = []
positions = []
for record in args.recording.values():
pos = Vector3r(*np.matmul(matrix, np.append(record.position, 1)))
poses.append(Pose(pos, record.orientation))
positions = [pos]
if args.axes:
client.simPlotTransforms(poses,
scale=100.0,
thickness=2.5,
is_persistent=True)
else:
client.simPlotPoints(positions,
args.color,
size=10,
is_persistent=True)
if args.lines:
client.simPlotLineStrip(positions,
args.color,
thickness=2.5,
is_persistent=True)
if args.aim:
line_list = []
for pose in poses:
line_list.append(pose.position)
x_axis, _, _ = AirSimNedTransform.local_axes_frame(pose)
line_list.append(pose.position + x_axis * 100)
client.simPlotLineList(line_list,
args.color,
thickness=2.5,
is_persistent=True)
def fly_zone(client: airsim.MultirotorClient,
zone: Rect,
altitude_shift: float = 0.0) -> None:
path = [
Vector3r(corner.x_val, corner.y_val, corner.z_val - altitude_shift)
for corner in zone.corners(repeat_first=True)
]
if AUGMENT_PATHS:
path = Controller.augment_path(path, max_dist=2)
if SHOW_PLOTS:
client.simPlotPoints(points=path,
is_persistent=True,
color_rgba=Rgba.White,
size=5)
client.simPlotLineStrip(points=path,
is_persistent=True,
color_rgba=Rgba.White)
# Controller.fly_path(client, path) ##client.moveOnPathAsync(path, velocity=2).join()
# XXX testing.. stretching Rect, zigzagging path and flying over it
zz_path = Rect(
Vector3r(0, 0, -altitude_shift) + zone.center,
# Vector3r(0, 0, -altitude_shift) + zone.half_width * 4,
# Vector3r(0, 0, -altitude_shift) + zone.half_height * 4,
zone.half_width * 2,
zone.half_height * 2,
).zigzag(4)
if AUGMENT_PATHS:
zz_path = Controller.augment_path(zz_path, max_dist=2)
if SHOW_PLOTS:
client.simPlotPoints(points=zz_path,
is_persistent=True,
color_rgba=Rgba.White,
size=5)
client.simPlotLineStrip(points=zz_path,
is_persistent=True,
color_rgba=Rgba.Green)
Controller.fly_path(
client, zz_path) ##client.moveOnPathAsync(zz_path, velocity=2).join()
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.flush:
client.simFlushPersistentMarkers()
def pose_from_meshroom_to_airsim(meshroom_pose):
assert len(meshroom_pose.center) == 3 and len(
meshroom_pose.rotation) == 9, meshroom_pose
xyzw = MeshroomTransform.rotation(meshroom_pose.rotation,
as_xyzw_quaternion=True)
return Pose(Vector3r(*meshroom_pose.center), Quaternionr(*xyzw))
poses = [
pose_from_meshroom_to_airsim(pose)
for pose in args.poses_dict.values()
]
positions = [pose.position for pose in poses]
if args.axes:
client.simPlotTransforms(poses,
scale=75.0,
thickness=2.5,
is_persistent=True)
else:
client.simPlotPoints(positions,
args.color,
size=10,
is_persistent=True)
if args.lines:
client.simPlotLineStrip(positions,
args.color,
thickness=2.5,
is_persistent=True)
if args.transformation:
meshroom_to_airsim = np.loadtxt(
args.transformation) # load the 4x4 transformation matrix
print(meshroom_to_airsim)
def align_meshroom_to_airsim(meshroom_pose,
meshroom_to_airsim_transform):
# NOTE this transformation is only based on the positions (and not on the orientations)
meshroom_pos = np.append(meshroom_pose.position.to_numpy_array(),
1) # [x, y, z, 1]
airsim_pos = np.matmul(meshroom_to_airsim_transform, meshroom_pos)
return Pose(Vector3r(*airsim_pos), meshroom_pose.orientation)
aligned_poses = [
align_meshroom_to_airsim(pose, meshroom_to_airsim)
for pose in poses
]
aligned_positions = [pose.position for pose in aligned_poses]
if args.axes:
client.simPlotTransforms(aligned_poses,
scale=75.0,
thickness=2.5,
is_persistent=True)
else:
client.simPlotPoints(aligned_positions,
args.color,
size=10,
is_persistent=True)
if args.lines:
client.simPlotLineStrip(positions,
args.color,
thickness=2.5,
is_persistent=True)
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}"')
