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 ImagePublisher(Node):
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 publish(self):
"""Publish images from AirSim to ROS"""
responses = self._airsim_client.simGetImages([
# uncompressed RGB array bytes
ImageRequest(self._camera_name, ImageType.Scene, compress=False),
# # infrared uncompressed image
# ImageRequest(self._camera_name, ImageType.Infrared, compress=False),
# # floating point uncompressed image
# ImageRequest(self._camera_name, ImageType.DepthPlanner, pixels_as_float=True, compress=False),
], self._vehicle_name)
color_response = responses[0]
# ir_response = responses[1]
# depth_response = responses[2]
header = Header()
header.stamp = self.get_clock().now().to_msg()
# TODO: implement parameter for frame id, also decide on if each separate image type should have a different frame id
# This may mean we should load the ids via ros parameters
header.frame_id = self._camera_frame_id
# Handle cam info it has not been found yet
if self._vehicle_name not in self._cam_info_msgs.keys():
self._cam_info_msgs[self._vehicle_name] = {}
cam_info = self._airsim_client.simGetCameraInfo(self._camera_name, self._vehicle_name)
d_params = self._airsim_client.simGetDistortionParams(self._camera_name, self._vehicle_name)
self.get_logger().info(f"{d_params}")
self.get_logger().info(f"""
HFOV: {cam_info.fov},
PROJ: {cam_info.proj_mat}
""")
# TODO: implement multiple cameras for each lens on realsense and update this method
self._cam_info_msgs[self._vehicle_name]["color"] = construct_info(header, cam_info, color_response.height, color_response.width)
# self._cam_info_msgs[self._vehicle_name]["ir"] = self._cam_info_msgs[self._vehicle_name]["color"]
image_color = construct_image(header, color_response, "bgr8")
# image_ir = construct_image(header, ir_response, "rgb8")
# image_depth = construct_image(header, depth_response, "rgb8")
# TODO: use camera pose from airsim
tfmsg = TransformStamped()
translation = Vector3(x=0., y=0., z=0.)
tfmsg.transform.translation = translation
tfmsg.transform.rotation = Quaternion(x=0., y=0., z=0., w=1.)
tfmsg.child_frame_id = self._camera_frame_id
tf_header = Header()
tf_header.stamp = header.stamp
tfmsg.header = tf_header
tfmsg.header.frame_id = "world"
self.br.sendTransform(tfmsg)
self._pub_color.publish(image_color)
# self._pub_ir.publish(image_ir)
# self._pub_depth.publish(image_depth)
self._pub_info_color.publish(self._cam_info_msgs[self._vehicle_name]["color"])