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.