Add shuttlecock 3D localizer node and dummy shuttlecock test script

.gitignore

@@ -16,3 +16,5 @@ log/
 autonomy/teleop/quest_teleop_server/certs/*
 datasets/
 outputs/
+
+modules/docker-compose.perception_override.yaml

autonomy/perception/package.xml

@@ -12,6 +12,7 @@
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
   <depend>image_transport</depend>
+  <depend>vision_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

autonomy/perception/perception/dummy_shuttlecock_publisher.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import CameraInfo, Image
+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):
+    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(
+            PointStamped, "/tracknetv3/shuttle_point", 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
+
+        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()
+        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_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)
+        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()

autonomy/perception/perception/shuttlecock_localizer_node.py

@@ -0,0 +1,171 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+# 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
+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):
+        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
+        )
+        visible_sub = message_filters.Subscriber(
+            self,
+            Bool,
+            "/tracknetv3/shuttle_visible"
+        )
+
+        det_sub = message_filters.Subscriber(
+            self,
+            PointStamped,
+            '/tracknetv3/shuttle_point'
+        )
+        depth_sub = message_filters.Subscriber(
+            self,
+            Image,
+            '/camera/depth/image_raw',
+            qos_profile=qos_profile_sensor_data
+        )
+
+        self.ts = ApproximateTimeSynchronizer(
+            [det_sub, visible_sub, depth_sub],
+            queue_size=10,
+            slop=0.015,
+            allow_headerless=True,
+        )
+
+        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: 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.point.x
+        v = det_msg.point.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()

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"
         ],
     },
 )

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.