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 get_UE4_coords(
client: airsim.MultirotorClient,
multirotor_state: airsim.MultirotorState = None
) -> Tuple[float, float, float]:
''' `(north, east, up)` values in `cm`, following the left-hand rule '''
if multirotor_state is None:
multirotor_state = client.getMultirotorState()
pos = multirotor_state.gps_location # GeoPoint
return pos.latitude, pos.longitude, pos.altitude
def get_NED_coords(
client: airsim.MultirotorClient,
multirotor_state: airsim.MultirotorState = None
) -> Tuple[float, float, float]:
''' `(north, east, down)` values in `m`, following the right-hand rule '''
if multirotor_state is None:
multirotor_state = client.getMultirotorState()
pos = multirotor_state.kinematics_estimated.position # Vector3r
return pos.x_val, pos.y_val, pos.z_val
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 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:
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)))
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