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 __move_on_box(client: airsim.MultirotorClient, z: float, side: float,
velocity: float) -> None:
# NOTE airsim.DrivetrainType.MaxDegreeOfFreedom allows controlling the drone yaw independently
# from the direction it is flying, this way we make it always point to the inside of the box
duration = side / velocity
vx_vy_yaw = [(velocity, 0, 90), (0, velocity, 180), (-velocity, 0, 270),
(0, -velocity, 0)]
print(f"[ff] Moving on box...", end=" ", flush=True)
for vx, vy, yaw in vx_vy_yaw:
client.simPrintLogMessage(
f"moving by velocity vx={vx}, vy={vy}, yaw={yaw}")
client.moveByVelocityZAsync(vx, vy, z, duration,
airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(False, yaw)).join()
time.sleep(4)
client.hoverAsync().join()
client.landAsync().join()
print(f"done.")
