def __init__(self, rate=60):
super().__init__("airsim_images")
# Create timer for calling publish at predefined rate
self.create_timer(1/rate, self.publish)
# AirSim variables
self._airsim_client = MultirotorClient(ip=os.environ["WSL_HOST_IP"])
# self._camera_name = "front_center"
self._camera_name = "bottom_center"
self._camera_frame_id = "realsense"
self._vehicle_name = self.get_namespace().split("/")[1]
# ROS Publishers
# self._pub_ir = self.create_publisher(Image, "ir/image_raw", 1)
self._pub_color = self.create_publisher(Image, "color/image_raw", 1)
# self._pub_depth = self.create_publisher(Image, "depth/image_raw", 1)
# self._pub_info_ir = self.create_publisher(CameraInfo, "ir/camera_info", 1)
self._pub_info_color = self.create_publisher(CameraInfo, "color/camera_info", 1)
# self._pub_depth = self.create_publisher(Image, "depth/image_raw", 1)
# TF related variables
self.br = TransformBroadcaster(self)
# CV
self.bridge = CvBridge()
# Internal variables
self._cam_info_msgs = {}
self.get_logger().info("Initialized image publisher")
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 __move_on_path(client: airsim.MultirotorClient, path: List[Vec3],
velocity: float) -> None:
print(f"[ff] Moving on path...", end=" ", flush=True)
client.moveOnPathAsync(
[airsim.Vector3r(coord.x, coord.y, coord.z) for coord in path],
velocity,
).join()
print(f"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 _move_by_vehicle_poses(client: airsim.MultirotorClient,
args: argparse.Namespace) -> None:
raise Warning("simSetVehiclePose() is meant for ComputerVision mode")
# NOTE check https://github.com/microsoft/AirSim/pull/2324
for position, orientation in args.viewpoints:
pose = airsim.Pose(airsim.Vector3r(*position),
airsim.Quaternionr(*orientation))
client.simSetVehiclePose(pose, ignore_collision=True)
client.hoverAsync().join()
time.sleep(2)
def fly_path(
client: airsim.MultirotorClient,
path: List[Vector3r],
velocity: float = 2.0,
timeout_sec: float = 3e38, # FIXME make this a constant
) -> None:
if USE_AIRSIM_HIGH_LEVEL_CONTROL:
client.moveOnPathAsync(path, velocity, timeout_sec).join()
else:
_fly_path2(client, path, velocity, timeout_sec)
def enter_edit_mode(client: airsim.MultirotorClient,
points: List[Vector3r]) -> None:
mode = EditMode.TRANSLATING
step = 1.0
total = {
EditMode.TRANSLATING:
Vector3r(0, 0, 0),
# EditMode.ROTATING: Vector3r(0, 0, 0),
EditMode.SCALING:
Vector3r(1, 1, 1),
}
try:
while True:
key = ord(airsim.wait_key())
if key == 224: # arrow keys
arrow_key = ArrowKey(ord(airsim.wait_key()))
client.simFlushPersistentMarkers()
if mode == EditMode.TRANSLATING:
delta = {
ArrowKey.Up: Vector3r(step, 0, 0),
ArrowKey.Down: Vector3r(-step, 0, 0),
ArrowKey.Left: Vector3r(0, -step, 0),
ArrowKey.Right: Vector3r(0, step, 0),
}[arrow_key]
points = [point + delta for point in points]
total[EditMode.TRANSLATING] += delta
elif mode == EditMode.SCALING:
factor = {
ArrowKey.Up: 1 + step * 0.1,
ArrowKey.Down: 1 - step * 0.1,
ArrowKey.Left: 1 - step * 0.1,
ArrowKey.Right: 1 + step * 0.1,
}[arrow_key]
points = [point * factor for point in points]
total[EditMode.SCALING] *= factor
else:
assert False # TODO handle other edit modes for rotating and scaling
client.simPlotPoints(points,
Rgba.Blue,
POINT_CLOUD_POINT_SIZE,
is_persistent=True)
elif key == 27: # esc
ff.log("TRANSLATING:", total[EditMode.TRANSLATING])
ff.log("SCALING:", total[EditMode.SCALING])
return
else:
if key == ord(b"["):
step /= 2.0
elif key == ord(b"]"):
step *= 2.0
elif key == ord(b"\t"):
mode = EditMode.next(mode)
ff.log(f"{mode=} {step=}")
except KeyboardInterrupt:
return
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_by_positions(client: airsim.MultirotorClient,
args: argparse.Namespace) -> None:
for position, orientation in args.viewpoints:
_pitch, _roll, yaw = airsim.to_eularian_angles(
airsim.Quaternionr(*orientation))
client.moveToPositionAsync(
*position,
args.flight_velocity,
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=yaw),
).join()
client.hoverAsync().join()
time.sleep(2)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.flush:
client.simFlushPersistentMarkers()
if not args.transformation:
poses = [
Pose(record.position, record.orientation)
for record in args.recording.values()
]
positions = [record.position for record in args.recording.values()]
else:
matrix = np.loadtxt(
args.transformation) # load the 4x4 transformation matrix
print(matrix)
poses = []
positions = []
for record in args.recording.values():
pos = Vector3r(*np.matmul(matrix, np.append(record.position, 1)))
poses.append(Pose(pos, record.orientation))
positions = [pos]
if args.axes:
client.simPlotTransforms(poses,
scale=100.0,
thickness=2.5,
is_persistent=True)
else:
client.simPlotPoints(positions,
args.color,
size=10,
is_persistent=True)
if args.lines:
client.simPlotLineStrip(positions,
args.color,
thickness=2.5,
is_persistent=True)
if args.aim:
line_list = []
for pose in poses:
line_list.append(pose.position)
x_axis, _, _ = AirSimNedTransform.local_axes_frame(pose)
line_list.append(pose.position + x_axis * 100)
client.simPlotLineList(line_list,
args.color,
thickness=2.5,
is_persistent=True)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
initial_pose = client.simGetVehiclePose()
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
points, poses, names = [], [], []
for position, orientation in args.viewpoints:
position = airsim.Vector3r(*position)
orientation = airsim.Quaternionr(*orientation) # xyzw
points.append(position)
poses.append(airsim.Pose(position, orientation))
names.append(
ff.to_xyz_str(position) + "\n" + ff.to_xyzw_str(orientation))
plot_linestrips = True # else plot_transforms
curr_press_keys = set()
key_combination = {keyboard.Key.space, keyboard.Key.shift}
def on_press(key):
nonlocal plot_linestrips, curr_press_keys, key_combination
if key in key_combination:
curr_press_keys.add(key)
if curr_press_keys == {keyboard.Key.space}:
if (plot_linestrips := not plot_linestrips):
client.simPlotLineStrip(points, thickness=3, duration=10)
else:
client.simPlotTransforms(poses,
thickness=3,
scale=40,
duration=10)
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()
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_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(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_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 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 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 _move_by_path(client: airsim.MultirotorClient,
args: argparse.Namespace) -> None:
path = [
airsim.Vector3r(*position)
for position, _orientation in args.viewpoints
]
future = client.moveOnPathAsync(
path,
args.flight_velocity,
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=-1.5), # FIXME
)
return future
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
initial_pose = client.simGetVehiclePose()
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
if args.flush:
client.simFlushPersistentMarkers()
# NOTE we have to use (-y, -x, -z), and not (x, -y, -z), because these
# files were exported from Unreal Engine as FBX with Force Front XAxis
BUILDING = False
STATUE = True
def convert_position(p, t, s):
if BUILDING or STATUE:
if t is not None: p += t
if s is not None: p *= s
x, y, z = map(float, p)
return Vector3r(-y, -x, -z)
else:
return convert_uavmvs_to_airsim_position(p, t, s)
points = [
# convert_uavmvs_to_airsim_position(_, translation=args.offset, scaling=args.scale)
convert_position(_, args.offset, args.scale)
for _ in args.points[::args.every_k]
]
client.simPlotPoints(points,
Rgba.Blue,
POINT_CLOUD_POINT_SIZE,
is_persistent=True)
if args.trajectory is not None:
camera_poses, camera_positions = [], []
for i, camera in enumerate(args.trajectory):
if not camera.spline_interpolated: # XXX
pose = convert_uavmvs_to_airsim_pose(camera=camera,
translation=args.offset,
scaling=args.scale)
# print(f"=== {i} ===")
# print(camera)
# print(pose)
camera_poses.append(pose)
camera_positions.append(pose.position)
# client.simPlotLineStrip(camera_positions, Rgba.Black, TRAJECTORY_THICKNESS, is_persistent=True)
# client.simPlotPoints(camera_positions, Rgba.White, CAMERA_POSE_SIZE, is_persistent=True)
client.simPlotTransforms(camera_poses,
10 * CAMERA_POSE_SIZE,
is_persistent=True)
if args.edit:
enter_edit_mode(client, points)
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.")
def fly_zone(client: airsim.MultirotorClient,
zone: Rect,
altitude_shift: float = 0.0) -> None:
path = [
Vector3r(corner.x_val, corner.y_val, corner.z_val - altitude_shift)
for corner in zone.corners(repeat_first=True)
]
if AUGMENT_PATHS:
path = Controller.augment_path(path, max_dist=2)
if SHOW_PLOTS:
client.simPlotPoints(points=path,
is_persistent=True,
color_rgba=Rgba.White,
size=5)
client.simPlotLineStrip(points=path,
is_persistent=True,
color_rgba=Rgba.White)
# Controller.fly_path(client, path) ##client.moveOnPathAsync(path, velocity=2).join()
# XXX testing.. stretching Rect, zigzagging path and flying over it
zz_path = Rect(
Vector3r(0, 0, -altitude_shift) + zone.center,
# Vector3r(0, 0, -altitude_shift) + zone.half_width * 4,
# Vector3r(0, 0, -altitude_shift) + zone.half_height * 4,
zone.half_width * 2,
zone.half_height * 2,
).zigzag(4)
if AUGMENT_PATHS:
zz_path = Controller.augment_path(zz_path, max_dist=2)
if SHOW_PLOTS:
client.simPlotPoints(points=zz_path,
is_persistent=True,
color_rgba=Rgba.White,
size=5)
client.simPlotLineStrip(points=zz_path,
is_persistent=True,
color_rgba=Rgba.Green)
Controller.fly_path(
client, zz_path) ##client.moveOnPathAsync(zz_path, velocity=2).join()
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(" (use negative z values to fly upwards)\n")
scene_objects = client.simListSceneObjects(
name_regex=args.name) # use '.*' to list all
if args.list_all:
print(client.simListSceneObjects())
if not scene_objects:
print(f"[ff] No object found with name '{args.name}'")
print(
"[ff] Object names containing it:",
client.simListSceneObjects(f".*{args.name}.*"),
)
elif len(scene_objects) > 1:
print(f"[ff] Multiple objects found with name '{args.name}':",
scene_objects)
else:
object_name = scene_objects[0]
start_pose = client.simGetObjectPose(object_name)
print("[ff] Starting pose:", pose2args(start_pose))
if args.verbose:
print(" ", pose2str(start_pose))
new_pose = start_pose
if args.absolute:
new_pose.position.x_val = args.x
new_pose.position.y_val = args.y
new_pose.position.z_val = args.z
else:
new_pose.position.x_val += args.x
new_pose.position.y_val += args.y
new_pose.position.z_val += args.z
print(f"[ff] Moving object '{object_name}'", end="")
success = client.simSetObjectPose(object_name, pose=new_pose)
print(" (successful)" if success else " (failed)")
end_pose = client.simGetObjectPose(object_name)
print("[ff] Ending pose:", pose2args(end_pose))
if args.verbose:
print(" ", pose2str(end_pose))
if not success:
print(
"\nWarning: Make sure the object has Mobility = 'Movable' in UE4"
)
def plot_xyz_axis(
client: airsim.MultirotorClient,
x_axis: Vector3r,
y_axis: Vector3r,
z_axis: Vector3r,
origin: Vector3r,
colors=RGB,
thickness=2.5,
) -> None:
client.simPlotArrows([origin], [origin + x_axis],
colors[0],
thickness,
is_persistent=True)
client.simPlotArrows([origin], [origin + y_axis],
colors[1],
thickness,
is_persistent=True)
client.simPlotArrows([origin], [origin + z_axis],
colors[2],
thickness,
is_persistent=True)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.flush:
client.simFlushPersistentMarkers()
def pose_from_meshroom_to_airsim(meshroom_pose):
assert len(meshroom_pose.center) == 3 and len(
meshroom_pose.rotation) == 9, meshroom_pose
xyzw = MeshroomTransform.rotation(meshroom_pose.rotation,
as_xyzw_quaternion=True)
return Pose(Vector3r(*meshroom_pose.center), Quaternionr(*xyzw))
poses = [
pose_from_meshroom_to_airsim(pose)
for pose in args.poses_dict.values()
]
positions = [pose.position for pose in poses]
if args.axes:
client.simPlotTransforms(poses,
scale=75.0,
thickness=2.5,
is_persistent=True)
else:
client.simPlotPoints(positions,
args.color,
size=10,
is_persistent=True)
if args.lines:
client.simPlotLineStrip(positions,
args.color,
thickness=2.5,
is_persistent=True)
if args.transformation:
meshroom_to_airsim = np.loadtxt(
args.transformation) # load the 4x4 transformation matrix
print(meshroom_to_airsim)
def align_meshroom_to_airsim(meshroom_pose,
meshroom_to_airsim_transform):
# NOTE this transformation is only based on the positions (and not on the orientations)
meshroom_pos = np.append(meshroom_pose.position.to_numpy_array(),
1) # [x, y, z, 1]
airsim_pos = np.matmul(meshroom_to_airsim_transform, meshroom_pos)
return Pose(Vector3r(*airsim_pos), meshroom_pose.orientation)
aligned_poses = [
align_meshroom_to_airsim(pose, meshroom_to_airsim)
for pose in poses
]
aligned_positions = [pose.position for pose in aligned_poses]
if args.axes:
client.simPlotTransforms(aligned_poses,
scale=75.0,
thickness=2.5,
is_persistent=True)
else:
client.simPlotPoints(aligned_positions,
args.color,
size=10,
is_persistent=True)
if args.lines:
client.simPlotLineStrip(positions,
args.color,
thickness=2.5,
is_persistent=True)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
initial_pose = client.simGetVehiclePose()
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
ff.log(client.simGetCameraInfo(camera_name=CAPTURE_CAMERA))
if args.flush or (args.capture_dir and not args.debug):
client.simFlushPersistentMarkers()
camera_poses = []
for camera in args.trajectory:
position = convert_uavmvs_to_airsim_position(
camera.position, translation=args.offset, scaling=args.scale
)
orientation = quaternion_orientation_from_eye_to_look_at(position, LOOK_AT_TARGET)
camera_poses.append(Pose(position, orientation))
if args.debug:
camera_positions = [pose.position for pose in camera_poses]
client.simPlotPoints(camera_positions, Rgba.Blue, is_persistent=True)
client.simPlotLineStrip(camera_positions, Rgba.Cyan, thickness=2.5, is_persistent=True)
airsim_record = []
def do_stuff_at_uavmvs_viewpoint(i, pose):
nonlocal client, camera_poses, airsim_record
log_string = f"({i}/{len(camera_poses)})"
p, q = pose.position, pose.orientation
if args.debug:
log_string += f" position = {to_xyz_str(p)}"
client.simPlotTransforms([pose], scale=100, is_persistent=True)
# client.simPlotArrows([p], [LOOK_AT_TARGET], Rgba.White, thickness=3.0, duration=10)
elif args.capture_dir:
path = f"{args.prefix}pose{args.suffix}_{i:0{len(str(len(camera_poses)))}}.png"
path = os.path.join(args.capture_dir, path)
airsim.write_png(path, AirSimImage.get_mono(client, CAPTURE_CAMERA))
log_string += f' saved image to "{path}"'
record_line = AirSimRecord.make_line_string(p, q, time_stamp=str(i), image_file=path)
airsim_record.append(record_line)
ff.log(log_string)
if IS_CV_MODE:
for i, camera_pose in enumerate(camera_poses):
client.simSetVehiclePose(camera_pose, ignore_collision=True)
do_stuff_at_uavmvs_viewpoint(i, camera_pose)
time.sleep(CV_SLEEP_SEC)
else:
client.moveToZAsync(z=-10, velocity=VELOCITY).join() # XXX avoid colliding on take off
client.hoverAsync().join()
mean_position_error = 0.0
for i, camera_pose in enumerate(camera_poses):
client.moveToPositionAsync(
*to_xyz_tuple(camera_pose.position),
velocity=VELOCITY,
drivetrain=DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=YawMode(is_rate=False, yaw_or_rate=YAW_N),
).join()
with pose_at_simulation_pause(client) as real_pose:
# NOTE when we pre-compute the viewpoint's camera orientation, we use the
# expected drone position, which (should be close, but) is not the actual
# drone position. Hence, we could experiment with using fake orientation:
# quaternion_orientation_from_eye_to_look_at(real_pose.position, LOOK_AT_TARGET)
fake_pose = Pose(real_pose.position, camera_pose.orientation)
client.simSetVehiclePose(fake_pose, ignore_collision=True)
client.simContinueForFrames(1) # NOTE ensures pose change
do_stuff_at_uavmvs_viewpoint(i, fake_pose)
client.simSetVehiclePose(real_pose, ignore_collision=True)
position_error = real_pose.position.distance_to(camera_pose.position)
mean_position_error += position_error
ff.log_debug(f"{position_error = }")
mean_position_error /= len(camera_poses)
ff.log_debug(f"{mean_position_error = }")
if airsim_record:
print_to_file = args.record_path is not None
file = open(args.record_path, "w") if print_to_file else None
print(AirSimRecord.make_header_string(), file=(file if print_to_file else sys.stdout))
for line in airsim_record:
print(line, file=(file if print_to_file else sys.stdout))
if print_to_file:
file.close()
ff.log_info(f'Saved AirSim record to "{args.record_path}"')
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)))
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 teleport(client: airsim.MultirotorClient,
to: Pose,
ignore_collision: bool = True) -> None:
# HACK see https://github.com/Microsoft/AirSim/issues/1618#issuecomment-689152817
client.simSetVehiclePose(to, ignore_collision)
client.moveToPositionAsync(*ff.to_xyz_tuple(to.position), velocity=1)