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()
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: 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:
if args.reset:
client.reset()
client.simFlushPersistentMarkers()
plot_pose(client, TEST_POSE)
plot_xyz_axis(client, X, Y, Z, origin=O)
with airsimy.pose_at_simulation_pause(client) as pose:
plot_pose(client, pose)
client.simPlotArrows([pose.position], [LOOK_AT_TARGET],
Rgba.White,
is_persistent=True)
# NOTE use x' = (LOOK_AT_TARGET - p) as the new x-axis (i.e. front vector),
# and project the current up/down vector (z-axis in AirSim) into the plane
# that is normal to x' at point p. This way we can get the remaining right
# vector by computing cross(down, front).
x_prime = LOOK_AT_TARGET - pose.position
_, _, z_axis = AirSimNedTransform.local_axes_frame(pose)
z_prime = airsimy.vector_projected_onto_plane(z_axis,
plane_normal=x_prime)
# NOTE don't forget to normalize! Not doing so will break the orientation below.
x_prime /= x_prime.get_length()
z_prime /= z_prime.get_length()
y_prime = z_prime.cross(x_prime)
plot_xyz_axis(
client,
x_prime * 1.25,
y_prime * 1.25,
z_prime * 1.25,
origin=pose.position,
colors=CMY,
thickness=1.5,
)
# Now, find the orientation that corresponds to the x'-y'-z' axis frame:
new_pose = Pose(
pose.position,
airsimy.quaternion_that_rotates_axes_frame(
source_xyz_axes=(X, Y, Z),
target_xyz_axes=(x_prime, y_prime, z_prime),
),
)
plot_pose(client, new_pose
) # NOTE this should be the same as the plot_xyz_axis above!
if args.set:
client.simSetVehiclePose(new_pose, ignore_collision=True)
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")
