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 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()
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:
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) -> None:
if args.verbose:
print(f"[ff] HomeGeoPoint:\n{client.getHomeGeoPoint()}\n")
print(f"[ff] VehiclePose:\n{client.simGetVehiclePose()}\n")
#print(f"[ff] MultirotorState:\n{client.getMultirotorState()}\n")
home_UE4 = Vec3.from_GeoPoint(client.getHomeGeoPoint())
ground_offset_NED = Vec3.from_Vector3r(client.simGetVehiclePose().position)
#assert ground_offset_NED.x == ground_offset_NED.y == 0 # assumes the drone is at PlayerStart
#player_start = Vec3.from_Vector3r(
# client.simGetObjectPose(client.simListSceneObjects("PlayerStart.*")[0]).position
#)
print(f"[ff] Taking off")
client.takeoffAsync(timeout_sec=8).join()
fligh_path = BLOCKS_PATH # TODO add option to change on argparse
print(f"[ff] Moving on path...", flush=True)
client.moveOnPathAsync(path=[
airsim.Vector3r(coord.x, coord.y, coord.z) for coord in fligh_path
],
velocity=5).join()
print(f"[ff] Landing", flush=True)
client.landAsync().join()
# print(f"[ff] Going home", flush=True)
# client.goHomeAsync().join()
print(f"[ff] Done")
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:
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:
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}"')
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