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..e3b2e496
--- /dev/null
+++ b/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()
diff --git a/autonomy/perception/perception/shuttlecock_localizer_node.py b/autonomy/perception/perception/shuttlecock_localizer_node.py
new file mode 100644
index 00000000..fcdd465d
--- /dev/null
+++ b/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()
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.