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(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")
class MyClient(MultirotorClient):
def __init__(self, **kwargs):
super(MyClient, self).__init__(**kwargs)
self.client = MultirotorClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.client.armDisarm(True)
#self.client.reset() # 실행 시 뒤에 코드가 무시될 수 있어 필요할 때만 주석 해제
self.client.takeoffAsync().join()
self.roll = PIDDouble()
self.pitch = PIDDouble()
# Set PID Gain
self.set_pid_gain()
#Save Flight Data
self.flight_data = pd.DataFrame()
self.flight_data['altitude'] = 0
self.now = None
# Drone Value
self.angle = {'pitch': None, 'roll': None, 'yaw': None}
self.rate = {'pitch': None, 'roll': None, 'yaw': None}
self.altitude = None
self.longitude = None
self.latitude = None
self.DT = 0.001
self.ccr = {1: None, 2: None, 3: None, 4: None, 'duration': 1}
self.motor = {1: 0.60001, 2: 0.60001, 3: 0.6, 4: 0.6, 'duration': 1}
def __del__(self):
self.now = time.localtime()
self.now = time.strftime('%d%H%M%S', self.now)
# self.flight_data.to_csv(f'data/simulation_data_{self.now}')
def run(self):
while 1:
start = time.time()
# Read Sensor Data
# IMU
self.read_angle()
self.read_rate()
# Barometer
self.read_altitude()
# GPS
self.read_gps()
# Save Data
self.flight_data['altitude'] = self.altitude
# PID Control
self.Double_Roll_Pitch_PID_Calculation(self.pitch, 0,
self.angle['pitch'],
self.rate['pitch'], self.DT)
self.Double_Roll_Pitch_PID_Calculation(self.roll, 0,
self.angle['roll'],
self.rate['roll'], self.DT)
# PWM
self.ccr[
1] = 84000 + 600 * 84 - self.pitch.IN.pid_result + self.roll.IN.pid_result
self.ccr[
2] = 84000 + 600 * 84 + self.pitch.IN.pid_result + self.roll.IN.pid_result
self.ccr[
3] = 84000 + 600 * 84 + self.pitch.IN.pid_result - self.roll.IN.pid_result
self.ccr[
4] = 84000 + 600 * 84 - self.pitch.IN.pid_result - self.roll.IN.pid_result
for i in range(4):
if self.ccr[i + 1] < 84000: self.ccr[i + 1] = 84000
elif self.ccr[i + 1] > 168000: self.ccr[i + 1] = 168000
self.ccr[i + 1] = (self.ccr[i + 1] - 84000) / 84000
# Write Motors
self.client.moveByMotorPWMsAsync(self.ccr[4], self.ccr[2],\
self.ccr[1], self.ccr[3], self.ccr['duration'])
# print('roll : ',self.angle['roll'],' pitch : ', self.angle['pitch'], ' yaw : ', self.angle['yaw'])
self.DT = time.time() - start
if keyboard.read_key() == 'q':
break
del self.client
def read_angle(self):
self.angle['pitch'], self.angle['roll'], self.angle['yaw'] = \
airsim.to_eularian_angles(self.client.getImuData('Imu', 'Drone1').orientation)
self.angle['pitch'] *= -1
def read_rate(self):
self.rate['roll'] = self.client.getImuData(
'Imu', 'Drone1').angular_velocity.x_val
self.rate['pitch'] = self.client.getImuData(
'Imu', 'Drone1').angular_velocity.y_val
self.rate['yaw'] = self.client.getImuData(
'Imu', 'Drone1').angular_velocity.z_val
self.rate['pitch'] *= -1
def read_altitude(self):
self.altitude = self.client.getBarometerData(
barometer_name='Barometer', vehicle_name='Drone1').altitude
def read_gps(self):
self.longitude = self.client.getGpsData(
'Gps', 'Drone1').gnss.geo_point.longitude
self.latitude = self.client.getGpsData(
'Gps', 'Drone1').gnss.geo_point.latitude
def set_pid_gain(self):
self.pitch.IN.kp = 104
self.pitch.IN.ki = 0
self.pitch.IN.kd = 7
self.pitch.OUT.kp = 14
self.pitch.OUT.ki = 0
self.pitch.OUT.kd = 3
self.roll.IN.kp = 104
self.roll.IN.ki = 0
self.roll.IN.kd = 7
self.roll.OUT.kp = 14
self.roll.OUT.ki = 0
self.roll.OUT.kd = 3
def Double_Roll_Pitch_PID_Calculation(
self,
axis: PIDDouble,
set_point_angle: float,
angle: float, # BNO080 Rotation Angle
rate: float, # ICM-20602 Angular Rate
DT: float # loop time
):
# *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************#
# P calculation
axis.OUT.reference = set_point_angle
axis.OUT.meas_value = angle
axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value
axis.OUT.p_result = axis.OUT.error * axis.OUT.kp
# I calculation, DT need to be given in every loop
axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop
OUT_ERR_SUM_MAX = 2000
OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX
if axis.OUT.error_sum > OUT_ERR_SUM_MAX:
axis.OUT.error_sum = OUT_ERR_SUM_MAX
elif axis.OUT.error_sum < OUT_ERR_SUM_MIN:
axis.OUT.error_sum = OUT_ERR_SUM_MIN
axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop
# D calculation
OUTER_DERIV_FILT_ENABLE = True
axis.OUT.error_deriv = -rate # Define derivative of outer loop (rate = ICM-20602 Angular Rate)
if not OUTER_DERIV_FILT_ENABLE:
axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop
else:
axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative
axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop
axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop
# ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************#
axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control)
axis.IN.meas_value = rate # ICM-20602 angular rate
# P calculation
axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop
axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop
# I calculation
axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop
IN_ERR_SUM_MAX = 500
IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX
if axis.IN.error_sum > IN_ERR_SUM_MAX:
axis.IN.error_sum = IN_ERR_SUM_MAX
elif axis.IN.error_sum < IN_ERR_SUM_MIN:
axis.IN.error_sum = IN_ERR_SUM_MIN
axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop
# D calculation
axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev
) / DT # Define derivative of inner loop
axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value
INNER_DERIV_FILT_ENABLE = True
if not INNER_DERIV_FILT_ENABLE:
axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop
else:
axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative
axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop
axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop
def Single_Yaw_Heading_PID_Calculation(
axis: PIDSingle,
set_point_angle: float,
angle: float, # BNO080 Rotation Angle
rate: float, # ICM-20602 Angular Rate
DT: float # loop time
):
# *********** Single PID Begin (Yaw Angular Position) *************#
axis.reference = set_point_angle # Set point of yaw heading @ yaw stick is center.
axis.meas_value = angle # Current BNO080_Yaw angle @ yaw stick is center.
# P Calculation
axis.error = axis.reference - axis.meas_value # Define error of yaw angle control
if (axis.error > 180):
axis.error -= 360
elif (axis.error < -180):
axis.error += 360
axis.p_result = axis.error * axis.kp # Calculate P result of yaw angle control
# I Calculation
axis.error_sum = axis.error_sum + axis.error * DT # Define summation of yaw angle control
axis.i_result = axis.error_sum * axis.ki # Calculate I result of yaw angle control
# D Calculation
axis.error_deriv = -rate # Define differentiation of yaw angle control
axis.d_result = axis.error_deriv * axis.kd # Calculate D result of yaw angle control
axis.pid_result = axis.p_result + axis.i_result + axis.d_result # Calculate PID result of yaw angle contro
def Single_Yaw_Rate_PID_Calculation(
axis: PIDSingle,
set_point_rate: float,
rate: float, # ICM-20602 Angular Rate
DT: float):
# *********** Single PID Begin (Yaw Angular Rate Control) *************#
axis.reference = set_point_rate # Set point of yaw heading @ yaw stick is not center.
axis.meas_value = rate # Current ICM20602.gyro_z @ yaw stick is not center.
# P calculation
axis.error = axis.reference - axis.meas_value # Define error of yaw rate control
axis.p_result = axis.error * axis.kp # Calculate P result of yaw rate control
# I calculation
axis.error_sum = axis.error_sum + axis.error * DT # Define summation of yaw rate control
axis.i_result = axis.error_sum * axis.ki # Calculate I result of yaw rate control
# D calculation
axis.error_deriv = -(
axis.meas_value - axis.meas_value_prev
) / DT # Define differentiation of yaw rate control
axis.meas_value_prev = axis.meas_value # Refresh value_prev to the latest value
axis.d_result = axis.error_deriv * axis.kd # Calculate D result of yaw rate control
axis.pid_result = axis.p_result + axis.i_result + axis.d_result # Calculate PID result of yaw control
# *******************************************************************#
def Double_Altitude_PID_Calculation(axis: PIDDouble,
set_point_altitude: float,
current_altitude: float, DT: float):
# *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************#
axis.OUT.reference = set_point_altitude # Set point of outer PID control
axis.OUT.meas_value = current_altitude # Actual Altitude from Fusion
# P Calculation
axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value # Define error of outer loop
axis.OUT.p_result = axis.OUT.error * axis.OUT.kp # Calculate P result of outer loop
# I Calculation
axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop
OUT_ERR_SUM_MAX = 500
OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX
if axis.OUT.error_sum > OUT_ERR_SUM_MAX:
axis.OUT.error_sum = OUT_ERR_SUM_MAX
elif axis.OUT.error_sum < OUT_ERR_SUM_MIN:
axis.OUT.error_sum = OUT_ERR_SUM_MIN
axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop
# D Calculation
axis.OUT.error_deriv = -(axis.OUT.meas_value -
axis.OUT.meas_value_prev) / DT
axis.OUT.meas_value_prev = axis.OUT.meas_value
OUTER_DERIV_FILT_ENABLE = True
if not OUTER_DERIV_FILT_ENABLE:
axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop
else:
axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative
axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop
axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop
# ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************#
axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control)
axis.IN.meas_value = -axis.OUT.error_deriv_filt # ICM-20602 angular rate
# P calculation
axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop
axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop
# I calculation
axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop
IN_ERR_SUM_MAX = 500
IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX
if axis.IN.error_sum > IN_ERR_SUM_MAX:
axis.IN.error_sum = IN_ERR_SUM_MAX
elif axis.IN.error_sum < IN_ERR_SUM_MIN:
axis.IN.error_sum = IN_ERR_SUM_MIN
axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop
# D calculation
axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev
) / DT # Define derivative of inner loop
axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value
INNER_DERIV_FILT_ENABLE = True
if not INNER_DERIV_FILT_ENABLE:
axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop
else:
axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative
axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop
axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop
if axis.IN.pid_result < -2100: axis.IN.pid_result = -2100
if axis.IN.pid_result > 16800: axis.IN.pid_result = 16800
def Double_GPS_PID_Calculation(axis: PIDDouble, set_point_gps: float,
gps: float, DT: float):
# *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************#
axis.OUT.reference = set_point_gps # Set point of outer PID control
axis.OUT.meas_value = gps
# P calculation
axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value # Define error of outer loop
axis.OUT.p_result = axis.OUT.error * axis.OUT.kp # Calculate P result of outer loop
# I calculation
axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop
OUT_ERR_SUM_MAX = 500
OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX
if axis.OUT.error_sum > OUT_ERR_SUM_MAX:
axis.OUT.error_sum = OUT_ERR_SUM_MAX
elif axis.OUT.error_sum < OUT_ERR_SUM_MIN:
axis.OUT.error_sum = OUT_ERR_SUM_MIN
axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop
# D calculation
axis.OUT.error_deriv = -(axis.OUT.meas_value - axis.OUT.meas_value_prev
) / DT # Define derivative of outer loop
axis.OUT.meas_value_prev = axis.OUT.meas_value
OUTER_DERIV_FILT_ENABLE = True
if not OUTER_DERIV_FILT_ENABLE:
axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop
else:
axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative
axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop
axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop
# ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************#
axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control)
axis.IN.meas_value = -axis.OUT.error_deriv
# P calculation
axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop
axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop
# I calculation
axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop
IN_ERR_SUM_MAX = 500
IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX
if axis.IN.error_sum > IN_ERR_SUM_MAX:
axis.IN.error_sum = IN_ERR_SUM_MAX
elif axis.IN.error_sum < IN_ERR_SUM_MIN:
axis.IN.error_sum = IN_ERR_SUM_MIN
axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop
# D calculation
axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev
) / DT # Define derivative of inner loop
axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value
INNER_DERIV_FILT_ENABLE = True
if not INNER_DERIV_FILT_ENABLE:
axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop
else:
axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative
axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop
axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop
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)))