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_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_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()
try:
import airsim
except ModuleNotFoundError:
airsim_path = ff.Default.AIRSIM_CLIENT_PATH
assert os.path.exists(os.path.join(airsim_path, "client.py")), airsim_path
sys.path.insert(0, os.path.dirname(airsim_path))
import airsim
###############################################################################
# constants ###################################################################
###############################################################################
# test paths for different maps (in NED coordinates)
BLOCKS_PATH = [
Vec3(15, 18, -40),
Vec3(55, 18, -40),
Vec3(55, -20, -40),
Vec3(15, -20, -40),
Vec3(15, 18, -40), # closed loop
]
###############################################################################
# preflight (called before connecting) ########################################
###############################################################################
def preflight(args: argparse.Namespace) -> None:
args.flight_velocity = 5
args.flight_path = BLOCKS_PATH # TODO add more options
def from_NED(coords_NED, ground_offset_NED, home_UE4):
''' Converts NED coordinates to Unreal system.
Assumes PlayerStart is at (0, 0, 0) in AirSim's local NED coordinate system. '''
# AirSim uses meters and +z aiming down
return Vec3.flip_z(100 * (coords_NED - ground_offset_NED) + home_UE4)
def to_NED(coords_UE4, ground_offset_NED, home_UE4):
''' Converts Unreal coordinates to NED system.
Assumes PlayerStart is at (0, 0, 0) in AirSim's local NED coordinate system. '''
# Unreal uses cm and +z aiming up
return Vec3.flip_z(0.01 * (coords_UE4 - home_UE4) + ground_offset_NED)
try:
import airsim
except ModuleNotFoundError:
pass # don't worry, it'll be imported later
# aliases for directories with downloaded environments
ENV_ROOT = {
"doc": "D:\\Documents\\AirSim", # .exe files
"dev": "D:\\dev\\AirSim\\Unreal\\Environments", # .sln and .uproject files
"custom": "D:\\dev\\UE4\\Custom Environments", # .sln and .uproject files
}
# test paths for different maps (in NED coordinates)
BLOCKS_PATH = [
Vec3(15, 15, -40),
Vec3(55, 15, -40),
Vec3(55, -20, -40),
Vec3(15, -20, -40),
]
#######################################################
#######################################################
def to_NED(coords_UE4, ground_offset_NED, home_UE4):
''' Converts Unreal coordinates to NED system.
Assumes PlayerStart is at (0, 0, 0) in AirSim's local NED coordinate system. '''
# Unreal uses cm and +z aiming up
return Vec3.flip_z(0.01 * (coords_UE4 - home_UE4) + ground_offset_NED)