From 4f2b80621903814f711bfd620ef2f910f1c67bf2 Mon Sep 17 00:00:00 2001 From: Allen Abraham Date: Fri, 3 Jul 2026 20:31:42 -0400 Subject: [PATCH 1/4] Add shuttlecock 3D localizer node and dummy shuttlecock test script --- .gitignore | 2 + autonomy/perception/package.xml | 1 + .../perception/dummy_shuttlecock_publisher.py | 81 ++++++++++ .../perception/shuttlecock_localizer_node.py | 152 ++++++++++++++++++ autonomy/perception/setup.py | 4 +- watod-config.sh | 2 +- 6 files changed, 240 insertions(+), 2 deletions(-) create mode 100644 autonomy/perception/perception/dummy_shuttlecock_publisher.py create mode 100644 autonomy/perception/perception/shuttlecock_localizer_node.py diff --git a/.gitignore b/.gitignore index 313d9463..c493cd82 100644 --- a/.gitignore +++ b/.gitignore @@ -16,3 +16,5 @@ log/ autonomy/teleop/quest_teleop_server/certs/* datasets/ outputs/ + +modules/docker-compose.perception_override.yaml diff --git a/autonomy/perception/package.xml b/autonomy/perception/package.xml index 521c709f..6925581c 100644 --- a/autonomy/perception/package.xml +++ b/autonomy/perception/package.xml @@ -12,6 +12,7 @@ geometry_msgs std_msgs image_transport + vision_msgs ament_copyright ament_flake8 diff --git a/autonomy/perception/perception/dummy_shuttlecock_publisher.py b/autonomy/perception/perception/dummy_shuttlecock_publisher.py new file mode 100644 index 00000000..41c4e912 --- /dev/null +++ b/autonomy/perception/perception/dummy_shuttlecock_publisher.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image +from vision_msgs.msg import Detection2D +from builtin_interfaces.msg import Time +from vision_msgs.msg import Detection2D, BoundingBox2D, Pose2D, Point2D +from cv_bridge import CvBridge +import numpy as np + + +class DummyShuttlecockPublisher(Node): + def __init__(self): + super().__init__("synthetic_shuttlecock_publisher") + + self.fx = self.fy = 500.0 + self.cx, self.cy = 320.0, 240.0 + self.u, self.v = 370.0, 290.0 + self.depth_mm = 2000 # 2.0m, will be read back as uint16 mm by the real node + self.bridge = CvBridge() + self.info_pub = self.create_publisher( + CameraInfo, "/camera/depth/camera_info", 10 + ) + self.det_pub = self.create_publisher( + Detection2D, "/perception/shuttlecock/detections_2d", 10 + ) + self.depth_pub = self.create_publisher(Image, "/camera/depth/image_raw", 10) + + self.timer = self.create_timer( + 0.5, self._publish_all + ) # 2Hz is plenty for a test + + def _publish_all(self): + stamp = self.get_clock().now().to_msg() + + # Build and Publish CameraInfo + info_msg = CameraInfo() + info_msg.header.stamp = stamp + info_msg.header.frame_id = "camera_link" + info_msg.width = 640 + info_msg.height = 480 + info_msg.k = [ + self.fx, 0.0, self.cx, + 0.0, self.fy, self.cy, + 0.0, 0.0, 1.0, + ] + self.info_pub.publish(info_msg) + + # Build and publish Detection2D using stamp + self.u/self.v + det_image = Detection2D() + det_image.header.stamp = stamp + det_image.header.frame_id = "camera_link" + det_image.bbox = BoundingBox2D() + det_image.bbox.center.position = Point2D() + det_image.bbox.center.position.x = self.u + det_image.bbox.center.position.y = self.v + self.det_pub.publish(det_image) + + # Build and publish Image (depth) using stamp + self.depth_mm + depth_array = np.full((480,640), self.depth_mm, dtype=np.uint16) + depth_msg = self.bridge.cv2_to_imgmsg(depth_array, encoding="16UC1") + depth_msg.header.stamp = stamp + depth_msg.header.frame_id = "camera_link" + self.depth_pub.publish(depth_msg) + +def main(args=None): + rclpy.init(args=args) + node = DummyShuttlecockPublisher() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/autonomy/perception/perception/shuttlecock_localizer_node.py b/autonomy/perception/perception/shuttlecock_localizer_node.py new file mode 100644 index 00000000..2f486bf7 --- /dev/null +++ b/autonomy/perception/perception/shuttlecock_localizer_node.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data # standard for tracking fast objects such as a birdie +import message_filters +from sensor_msgs.msg import Image, CameraInfo +from vision_msgs.msg import Detection2D +from cv_bridge import CvBridge +from message_filters import ApproximateTimeSynchronizer +import numpy as np +from geometry_msgs.msg import PointStamped, Point + +class ShuttlecockLocalizer(Node): + def __init__(self): + super().__init__('shuttlecock_localizer_node') + self.bridge = CvBridge() + # holding (fx, fy, cx, cy) until CameraInfo arrives + self.intrinsics = None + + self.create_subscription( + CameraInfo, + '/camera/depth/camera_info', + self._store_intrinsics, + qos_profile_sensor_data, + ) + + self.pub = self.create_publisher( + PointStamped, + '/perception/shuttlecock/detections_3d', + 10 + ) + + det_sub = message_filters.Subscriber( + self, + Detection2D, + '/perception/shuttlecock/detections_2d' + ) + depth_sub = message_filters.Subscriber( + self, + Image, + '/camera/depth/image_raw', + qos_profile = qos_profile_sensor_data + ) + + self.ts = ApproximateTimeSynchronizer( + [det_sub, depth_sub], + queue_size = 10, + slop = 0.015 + ) + + self.ts.registerCallback(self._synchronized_callback) + + def _store_intrinsics(self, msg: CameraInfo): + # 3x3 intrinsic matrix + k = msg.k + self.intrinsics = (k[0], k[4], k[2], k[5]) # fx, fy, cx, cy + self.get_logger().info( + f"Got intrinsics: fx={k[0]:.1f} fy={k[4]:.1f} cx={k[2]:.1f} cy={k[5]:.1f}" + ) + + def _synchronized_callback(self, det_msg: Detection2D, depth_msg: Image): + # Checking if intrinsics have been calculated first + if self.intrinsics is None: + self.get_logger().warn("Waiting for camera intrinsics...", throttle_duration_sec = 2.0) + return + + # extract 2d coordinates + u = det_msg.bbox.center.position.x + v = det_msg.bbox.center.position.y + + # convert ROS2 image message to numpy array + try: + depth_img = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding = "passthrough") + except Exception as e: + self.get_logger().error(f"Failed to convert depth image: {str(e)}") + return + + # sample depth array around u,v patch + z_cam = self._sample_depth(depth_img, u, v) + + # bail if depth sample returns none (invalid patch) + if z_cam is None: + self.get_logger().warn( + "Birdie detected, but depth patch is invalid (too many 0s/NaNs).", + throttle_duration_sec=1.0, + ) + return + + fx, fy, cx, cy = self.intrinsics + + x_cam = (u - cx) * z_cam / fx + y_cam = (v - cy) * z_cam / fy + + point_msg = PointStamped() + point_msg.header.stamp = det_msg.header.stamp + point_msg.header.frame_id = "camera_link" + point_msg.point = Point(x = x_cam, y = y_cam, z = z_cam) + self.pub.publish(point_msg) + + self.get_logger().info( + f"Successfully synced Detection (Time: {det_msg.header.stamp.sec}) " + " " + f"with Depth Image (Time: {depth_msg.header.stamp.sec})" + " " + f"Birdie Position: X = {x_cam:.3f}m, Y = {y_cam:.3f}m, Z = {z_cam:.3f}m" + ) + + def _sample_depth(self, depth_img, u: float, v: float): + # median-sample around a small patch around (u.v). Returns depth in meters or none if the patch is invalid + r = 2 # patch radius + h, w = depth_img.shape[:2] + ui, vi = int(round(u)), int(round(v)) + + # slice out depth image + u_min = max(0, ui - r) + u_max = min(w, ui + r + 1) + v_min = max(0, vi - r) + v_max = min(h, vi + r + 1) + + patch = depth_img[v_min:v_max, u_min:u_max] + + # if requested pixel is outside of the frame + if patch.size == 0: + return None + + # keep valid numbers only + valid_mask = (patch > 0) & (~np.isnan(patch)) + valid_values = patch[valid_mask] + + # reject if less then half of the pixels in the matrix + if len(valid_values) < 0.5 * patch.size: + return None + + # take the median and convert from mm to m + median_depth_mm = np.median(valid_values) + + return float(median_depth_mm/1000) + +def main(args=None): + rclpy.init(args=args) + node = ShuttlecockLocalizer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/autonomy/perception/setup.py b/autonomy/perception/setup.py index 2c8df239..f720bf70 100644 --- a/autonomy/perception/setup.py +++ b/autonomy/perception/setup.py @@ -34,7 +34,9 @@ entry_points={ "console_scripts": [ "dummy_publisher_node = perception.dummy_publisher_node:main", - "pose_estimation_node = perception.pose_estimation_node:main" + "pose_estimation_node = perception.pose_estimation_node:main", + "dummy_shuttlecock_publisher = perception.dummy_shuttlecock_publisher:main", + "shuttlecock_localizer_node = perception.shuttlecock_localizer_node:main" ], }, ) diff --git a/watod-config.sh b/watod-config.sh index dfeeae29..54101480 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -19,7 +19,7 @@ ## - samples : starts sample ROS2 pubsub nodes ## - teleop : starts teleop nodes -ACTIVE_MODULES="teleop" +ACTIVE_MODULES="teleop perception" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. From 01ff62c253f463635a16ee78c634c0f638d8268b Mon Sep 17 00:00:00 2001 From: Allen Abraham Date: Sat, 4 Jul 2026 20:41:48 -0400 Subject: [PATCH 2/4] matching tracknet messages --- .../perception/dummy_shuttlecock_publisher.py | 30 +++++++++++-------- .../perception/shuttlecock_localizer_node.py | 27 +++++++++++------ 2 files changed, 36 insertions(+), 21 deletions(-) diff --git a/autonomy/perception/perception/dummy_shuttlecock_publisher.py b/autonomy/perception/perception/dummy_shuttlecock_publisher.py index 41c4e912..15a550fd 100644 --- a/autonomy/perception/perception/dummy_shuttlecock_publisher.py +++ b/autonomy/perception/perception/dummy_shuttlecock_publisher.py @@ -3,11 +3,10 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import CameraInfo, Image -from vision_msgs.msg import Detection2D -from builtin_interfaces.msg import Time -from vision_msgs.msg import Detection2D, BoundingBox2D, Pose2D, Point2D from cv_bridge import CvBridge import numpy as np +from geometry_msgs.msg import PointStamped, Point +from std_msgs.msg import Bool class DummyShuttlecockPublisher(Node): @@ -23,7 +22,7 @@ def __init__(self): CameraInfo, "/camera/depth/camera_info", 10 ) self.det_pub = self.create_publisher( - Detection2D, "/perception/shuttlecock/detections_2d", 10 + PointStamped, "/tracknetv3/shuttle_point", 10 ) self.depth_pub = self.create_publisher(Image, "/camera/depth/image_raw", 10) @@ -31,8 +30,16 @@ def __init__(self): 0.5, self._publish_all ) # 2Hz is plenty for a test + self.visible_pub = self.create_publisher( + Bool, "/tracknetv3/shuttle_visible", 10 + ) + def _publish_all(self): stamp = self.get_clock().now().to_msg() + + visible_msg = Bool() + visible_msg.data = True + self.visible_pub.publish(visible_msg) # Build and Publish CameraInfo info_msg = CameraInfo() @@ -48,14 +55,13 @@ def _publish_all(self): self.info_pub.publish(info_msg) # Build and publish Detection2D using stamp + self.u/self.v - det_image = Detection2D() - det_image.header.stamp = stamp - det_image.header.frame_id = "camera_link" - det_image.bbox = BoundingBox2D() - det_image.bbox.center.position = Point2D() - det_image.bbox.center.position.x = self.u - det_image.bbox.center.position.y = self.v - self.det_pub.publish(det_image) + det_msg = PointStamped() + det_msg.header.stamp = stamp + det_msg.header.frame_id = "camera_link" + det_msg.point.x = self.u + det_msg.point.y = self.v + det_msg.point.z = 0.0 + self.det_pub.publish(det_msg) # Build and publish Image (depth) using stamp + self.depth_mm depth_array = np.full((480,640), self.depth_mm, dtype=np.uint16) diff --git a/autonomy/perception/perception/shuttlecock_localizer_node.py b/autonomy/perception/perception/shuttlecock_localizer_node.py index 2f486bf7..3f880df8 100644 --- a/autonomy/perception/perception/shuttlecock_localizer_node.py +++ b/autonomy/perception/perception/shuttlecock_localizer_node.py @@ -5,11 +5,11 @@ from rclpy.qos import qos_profile_sensor_data # standard for tracking fast objects such as a birdie import message_filters from sensor_msgs.msg import Image, CameraInfo -from vision_msgs.msg import Detection2D from cv_bridge import CvBridge from message_filters import ApproximateTimeSynchronizer import numpy as np from geometry_msgs.msg import PointStamped, Point +from std_msgs.msg import Bool class ShuttlecockLocalizer(Node): def __init__(self): @@ -25,16 +25,22 @@ def __init__(self): qos_profile_sensor_data, ) + self.pub = self.create_publisher( PointStamped, '/perception/shuttlecock/detections_3d', 10 ) + visible_sub = message_filters.Subscriber( + self, + Bool, + "/tracknetv3/shuttle_visible" + ) det_sub = message_filters.Subscriber( self, - Detection2D, - '/perception/shuttlecock/detections_2d' + PointStamped, + '/tracknetv3/shuttle_point' ) depth_sub = message_filters.Subscriber( self, @@ -44,9 +50,9 @@ def __init__(self): ) self.ts = ApproximateTimeSynchronizer( - [det_sub, depth_sub], - queue_size = 10, - slop = 0.015 + [det_sub, visible_sub, depth_sub], + queue_size=10, + slop=0.015 ) self.ts.registerCallback(self._synchronized_callback) @@ -59,15 +65,18 @@ def _store_intrinsics(self, msg: CameraInfo): f"Got intrinsics: fx={k[0]:.1f} fy={k[4]:.1f} cx={k[2]:.1f} cy={k[5]:.1f}" ) - def _synchronized_callback(self, det_msg: Detection2D, depth_msg: Image): + def _synchronized_callback(self, det_msg: PointStamped, visible_msg: Bool, depth_msg: Image): # Checking if intrinsics have been calculated first if self.intrinsics is None: self.get_logger().warn("Waiting for camera intrinsics...", throttle_duration_sec = 2.0) return + + if not visible_msg.data: + return # extract 2d coordinates - u = det_msg.bbox.center.position.x - v = det_msg.bbox.center.position.y + u = det_msg.point.x + v = det_msg.point.y # convert ROS2 image message to numpy array try: From c23ab93c577b4f0c8bdc1dfe3e6ae187578775ad Mon Sep 17 00:00:00 2001 From: Allen Abraham Date: Sat, 4 Jul 2026 20:51:48 -0400 Subject: [PATCH 3/4] linting and allowing headerless files --- .../perception/dummy_shuttlecock_publisher.py | 14 ++-- .../perception/shuttlecock_localizer_node.py | 73 +++++++++++-------- 2 files changed, 52 insertions(+), 35 deletions(-) diff --git a/autonomy/perception/perception/dummy_shuttlecock_publisher.py b/autonomy/perception/perception/dummy_shuttlecock_publisher.py index 15a550fd..e3b2e496 100644 --- a/autonomy/perception/perception/dummy_shuttlecock_publisher.py +++ b/autonomy/perception/perception/dummy_shuttlecock_publisher.py @@ -24,7 +24,8 @@ def __init__(self): self.det_pub = self.create_publisher( PointStamped, "/tracknetv3/shuttle_point", 10 ) - self.depth_pub = self.create_publisher(Image, "/camera/depth/image_raw", 10) + self.depth_pub = self.create_publisher( + Image, "/camera/depth/image_raw", 10) self.timer = self.create_timer( 0.5, self._publish_all @@ -40,10 +41,10 @@ def _publish_all(self): visible_msg = Bool() visible_msg.data = True self.visible_pub.publish(visible_msg) - + # Build and Publish CameraInfo info_msg = CameraInfo() - info_msg.header.stamp = stamp + info_msg.header.stamp = stamp info_msg.header.frame_id = "camera_link" info_msg.width = 640 info_msg.height = 480 @@ -62,14 +63,15 @@ def _publish_all(self): det_msg.point.y = self.v det_msg.point.z = 0.0 self.det_pub.publish(det_msg) - + # Build and publish Image (depth) using stamp + self.depth_mm - depth_array = np.full((480,640), self.depth_mm, dtype=np.uint16) + depth_array = np.full((480, 640), self.depth_mm, dtype=np.uint16) depth_msg = self.bridge.cv2_to_imgmsg(depth_array, encoding="16UC1") depth_msg.header.stamp = stamp depth_msg.header.frame_id = "camera_link" self.depth_pub.publish(depth_msg) + def main(args=None): rclpy.init(args=args) node = DummyShuttlecockPublisher() @@ -84,4 +86,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/autonomy/perception/perception/shuttlecock_localizer_node.py b/autonomy/perception/perception/shuttlecock_localizer_node.py index 3f880df8..de448251 100644 --- a/autonomy/perception/perception/shuttlecock_localizer_node.py +++ b/autonomy/perception/perception/shuttlecock_localizer_node.py @@ -2,7 +2,8 @@ import rclpy from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data # standard for tracking fast objects such as a birdie +# standard for tracking fast objects such as a birdie +from rclpy.qos import qos_profile_sensor_data import message_filters from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge @@ -11,24 +12,24 @@ from geometry_msgs.msg import PointStamped, Point from std_msgs.msg import Bool + class ShuttlecockLocalizer(Node): def __init__(self): super().__init__('shuttlecock_localizer_node') self.bridge = CvBridge() - # holding (fx, fy, cx, cy) until CameraInfo arrives + # holding (fx, fy, cx, cy) until CameraInfo arrives self.intrinsics = None self.create_subscription( - CameraInfo, + CameraInfo, '/camera/depth/camera_info', - self._store_intrinsics, + self._store_intrinsics, qos_profile_sensor_data, ) - self.pub = self.create_publisher( PointStamped, - '/perception/shuttlecock/detections_3d', + '/perception/shuttlecock/detections_3d', 10 ) visible_sub = message_filters.Subscriber( @@ -43,16 +44,17 @@ def __init__(self): '/tracknetv3/shuttle_point' ) depth_sub = message_filters.Subscriber( - self, + self, Image, '/camera/depth/image_raw', - qos_profile = qos_profile_sensor_data + qos_profile=qos_profile_sensor_data ) self.ts = ApproximateTimeSynchronizer( [det_sub, visible_sub, depth_sub], queue_size=10, - slop=0.015 + slop=0.015, + allow_headerless=True, ) self.ts.registerCallback(self._synchronized_callback) @@ -62,15 +64,24 @@ def _store_intrinsics(self, msg: CameraInfo): k = msg.k self.intrinsics = (k[0], k[4], k[2], k[5]) # fx, fy, cx, cy self.get_logger().info( - f"Got intrinsics: fx={k[0]:.1f} fy={k[4]:.1f} cx={k[2]:.1f} cy={k[5]:.1f}" - ) + f"Got intrinsics: fx={ + k[0]:.1f} fy={ + k[4]:.1f} cx={ + k[2]:.1f} cy={ + k[5]:.1f}") - def _synchronized_callback(self, det_msg: PointStamped, visible_msg: Bool, depth_msg: Image): + def _synchronized_callback( + self, + det_msg: PointStamped, + visible_msg: Bool, + depth_msg: Image): # Checking if intrinsics have been calculated first if self.intrinsics is None: - self.get_logger().warn("Waiting for camera intrinsics...", throttle_duration_sec = 2.0) + self.get_logger().warn( + "Waiting for camera intrinsics...", + throttle_duration_sec=2.0) return - + if not visible_msg.data: return @@ -80,7 +91,8 @@ def _synchronized_callback(self, det_msg: PointStamped, visible_msg: Bool, depth # convert ROS2 image message to numpy array try: - depth_img = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding = "passthrough") + depth_img = self.bridge.imgmsg_to_cv2( + depth_msg, desired_encoding="passthrough") except Exception as e: self.get_logger().error(f"Failed to convert depth image: {str(e)}") return @@ -104,20 +116,21 @@ def _synchronized_callback(self, det_msg: PointStamped, visible_msg: Bool, depth point_msg = PointStamped() point_msg.header.stamp = det_msg.header.stamp point_msg.header.frame_id = "camera_link" - point_msg.point = Point(x = x_cam, y = y_cam, z = z_cam) + point_msg.point = Point(x=x_cam, y=y_cam, z=z_cam) self.pub.publish(point_msg) self.get_logger().info( - f"Successfully synced Detection (Time: {det_msg.header.stamp.sec}) " - " " - f"with Depth Image (Time: {depth_msg.header.stamp.sec})" - " " - f"Birdie Position: X = {x_cam:.3f}m, Y = {y_cam:.3f}m, Z = {z_cam:.3f}m" - ) + f"Successfully synced Detection (Time: { + det_msg.header.stamp.sec}) " " " f"with Depth Image (Time: { + depth_msg.header.stamp.sec})" " " f"Birdie Position: X = { + x_cam:.3f}m, Y = { + y_cam:.3f}m, Z = { + z_cam:.3f}m") def _sample_depth(self, depth_img, u: float, v: float): - # median-sample around a small patch around (u.v). Returns depth in meters or none if the patch is invalid - r = 2 # patch radius + # median-sample around a small patch around (u.v). Returns depth in + # meters or none if the patch is invalid + r = 2 # patch radius h, w = depth_img.shape[:2] ui, vi = int(round(u)), int(round(v)) @@ -132,7 +145,7 @@ def _sample_depth(self, depth_img, u: float, v: float): # if requested pixel is outside of the frame if patch.size == 0: return None - + # keep valid numbers only valid_mask = (patch > 0) & (~np.isnan(patch)) valid_values = patch[valid_mask] @@ -140,12 +153,13 @@ def _sample_depth(self, depth_img, u: float, v: float): # reject if less then half of the pixels in the matrix if len(valid_values) < 0.5 * patch.size: return None - + # take the median and convert from mm to m median_depth_mm = np.median(valid_values) - return float(median_depth_mm/1000) - + return float(median_depth_mm / 1000) + + def main(args=None): rclpy.init(args=args) node = ShuttlecockLocalizer() @@ -157,5 +171,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() + if __name__ == "__main__": - main() \ No newline at end of file + main() From c172e13efcf902392abc6a12c61c7d3fd5127e91 Mon Sep 17 00:00:00 2001 From: Allen Abraham Date: Sat, 4 Jul 2026 20:59:35 -0400 Subject: [PATCH 4/4] Fix f-string formatting for Python 3.10 compatibility --- .../perception/shuttlecock_localizer_node.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/autonomy/perception/perception/shuttlecock_localizer_node.py b/autonomy/perception/perception/shuttlecock_localizer_node.py index de448251..fcdd465d 100644 --- a/autonomy/perception/perception/shuttlecock_localizer_node.py +++ b/autonomy/perception/perception/shuttlecock_localizer_node.py @@ -64,11 +64,8 @@ def _store_intrinsics(self, msg: CameraInfo): k = msg.k self.intrinsics = (k[0], k[4], k[2], k[5]) # fx, fy, cx, cy self.get_logger().info( - f"Got intrinsics: fx={ - k[0]:.1f} fy={ - k[4]:.1f} cx={ - k[2]:.1f} cy={ - k[5]:.1f}") + f"Got intrinsics: fx={k[0]:.1f} fy={k[4]:.1f} cx={k[2]:.1f} cy={k[5]:.1f}" + ) def _synchronized_callback( self, @@ -120,12 +117,10 @@ def _synchronized_callback( self.pub.publish(point_msg) self.get_logger().info( - f"Successfully synced Detection (Time: { - det_msg.header.stamp.sec}) " " " f"with Depth Image (Time: { - depth_msg.header.stamp.sec})" " " f"Birdie Position: X = { - x_cam:.3f}m, Y = { - y_cam:.3f}m, Z = { - z_cam:.3f}m") + f"Successfully synced Detection (Time: {det_msg.header.stamp.sec}) " + f"with Depth Image (Time: {depth_msg.header.stamp.sec}) " + f"Birdie Position: X={x_cam:.3f}m, Y={y_cam:.3f}m, Z={z_cam:.3f}m" + ) def _sample_depth(self, depth_img, u: float, v: float): # median-sample around a small patch around (u.v). Returns depth in