diff --git a/src/subsystems/navigation/nav_bringup/launch/nav_bringup.launch.py b/src/subsystems/navigation/nav_bringup/launch/nav_bringup.launch.py index 6ca05525..558d3564 100644 --- a/src/subsystems/navigation/nav_bringup/launch/nav_bringup.launch.py +++ b/src/subsystems/navigation/nav_bringup/launch/nav_bringup.launch.py @@ -92,6 +92,12 @@ def generate_launch_description(): }.items(), ) + camera_relay_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('usb_camera_relay'), 'launch', 'camera_relay.launch.py' + )), + ) + return LaunchDescription([ DeclareLaunchArgument( 'sim', @@ -146,5 +152,6 @@ def generate_launch_description(): robot_state_publisher, mag_heading_launch, navigation_launch, + camera_relay_launch, config_gui, ]) diff --git a/src/subsystems/navigation/usb_camera_relay/config/camera_relay_params.yaml b/src/subsystems/navigation/usb_camera_relay/config/camera_relay_params.yaml new file mode 100644 index 00000000..9682d663 --- /dev/null +++ b/src/subsystems/navigation/usb_camera_relay/config/camera_relay_params.yaml @@ -0,0 +1,13 @@ +usb_camera_relay: + ros__parameters: + # List specific device paths to avoid accidentally grabbing ZED nodes. + # Leave as [''] (single empty string) to auto-discover all /dev/video* devices. + devices: + - /dev/video0 # USB cam 0 (front) + # - /dev/video3 # USB cam 1 (left side) + # - /dev/video5 # USB cam 2 (right side) + + fps: 10.0 + width: 640 + height: 480 + topic_prefix: /cameras diff --git a/src/subsystems/navigation/usb_camera_relay/launch/camera_relay.launch.py b/src/subsystems/navigation/usb_camera_relay/launch/camera_relay.launch.py new file mode 100644 index 00000000..48050c93 --- /dev/null +++ b/src/subsystems/navigation/usb_camera_relay/launch/camera_relay.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + default_params = os.path.join( + get_package_share_directory('usb_camera_relay'), 'config', 'camera_relay_params.yaml' + ) + + params_file = LaunchConfiguration('params_file') + + return LaunchDescription([ + DeclareLaunchArgument( + 'params_file', + default_value=default_params, + description='Full path to the camera relay params YAML', + ), + Node( + package='usb_camera_relay', + executable='camera_relay', + name='usb_camera_relay', + output='screen', + parameters=[params_file], + ), + ]) diff --git a/src/subsystems/navigation/usb_camera_relay/package.xml b/src/subsystems/navigation/usb_camera_relay/package.xml new file mode 100644 index 00000000..930dcf27 --- /dev/null +++ b/src/subsystems/navigation/usb_camera_relay/package.xml @@ -0,0 +1,18 @@ + + + usb_camera_relay + 0.0.0 + Publishes USB camera streams as sensor_msgs/Image ROS topics + UMD Loop + MIT + + ament_python + + rclpy + sensor_msgs + cv_bridge + + + ament_python + + diff --git a/src/subsystems/navigation/usb_camera_relay/resource/usb_camera_relay b/src/subsystems/navigation/usb_camera_relay/resource/usb_camera_relay new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/navigation/usb_camera_relay/setup.cfg b/src/subsystems/navigation/usb_camera_relay/setup.cfg new file mode 100644 index 00000000..e54b43a2 --- /dev/null +++ b/src/subsystems/navigation/usb_camera_relay/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/usb_camera_relay + +[install] +install_scripts=$base/lib/usb_camera_relay diff --git a/src/subsystems/navigation/usb_camera_relay/setup.py b/src/subsystems/navigation/usb_camera_relay/setup.py new file mode 100644 index 00000000..03fbf33e --- /dev/null +++ b/src/subsystems/navigation/usb_camera_relay/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup +from glob import glob +import os + +package_name = 'usb_camera_relay' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='UMD Loop', + maintainer_email='todo@todo.com', + description='Publishes USB camera streams as sensor_msgs/Image ROS topics', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'camera_relay = usb_camera_relay.camera_relay_node:main', + ], + }, +) diff --git a/src/subsystems/navigation/usb_camera_relay/usb_camera_relay/__init__.py b/src/subsystems/navigation/usb_camera_relay/usb_camera_relay/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/navigation/usb_camera_relay/usb_camera_relay/camera_relay_node.py b/src/subsystems/navigation/usb_camera_relay/usb_camera_relay/camera_relay_node.py new file mode 100644 index 00000000..94475ea4 --- /dev/null +++ b/src/subsystems/navigation/usb_camera_relay/usb_camera_relay/camera_relay_node.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python3 + +import os +import time +import threading +from typing import List, Optional + +import cv2 +from cv_bridge import CvBridge + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image + + +def _discover_devices(max_index: int = 10) -> List[str]: + """Scan /dev/video0..N and return paths that produce frames.""" + found = [] + for i in range(max_index): + path = f"/dev/video{i}" + if not os.path.exists(path): + continue + cap = cv2.VideoCapture(i, cv2.CAP_V4L2) + if cap.isOpened(): + ret, _ = cap.read() + if ret: + found.append(path) + cap.release() + return found + + +class UsbCameraRelayNode(Node): + def __init__(self) -> None: + super().__init__("usb_camera_relay") + + self.declare_parameter("devices", [""]) + self.declare_parameter("fps", 10.0) + self.declare_parameter("width", 640) + self.declare_parameter("height", 480) + self.declare_parameter("topic_prefix", "/cameras") + + raw_devices: List[str] = list(self.get_parameter("devices").value) + self._fps: float = float(self.get_parameter("fps").value) + self._width: int = int(self.get_parameter("width").value) + self._height: int = int(self.get_parameter("height").value) + prefix: str = str(self.get_parameter("topic_prefix").value) + + self._bridge = CvBridge() + self._running = True + self._threads: List[threading.Thread] = [] + + if not raw_devices or raw_devices == [""]: + self.get_logger().info("No devices specified — auto-discovering cameras...") + devices = _discover_devices() + if not devices: + self.get_logger().error("No USB cameras found. Check /dev/video* devices.") + return + self.get_logger().info(f"Discovered: {devices}") + else: + devices = raw_devices + + for idx, device in enumerate(devices): + topic = f"{prefix}/cam{idx}/image_raw" + pub = self.create_publisher(Image, topic, 5) + t = threading.Thread( + target=self._capture_loop, + args=(device, idx, pub), + daemon=True, + ) + self._threads.append(t) + t.start() + + def _capture_loop(self, device: str, idx: int, pub) -> None: + cap = cv2.VideoCapture(device, cv2.CAP_V4L2) + if not cap.isOpened(): + self.get_logger().error(f"[cam {idx}] Cannot open {device}") + return + + cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._width) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._height) + + self.get_logger().info(f"[cam {idx}] {device} → {pub.topic_name}") + + interval = 1.0 / self._fps + + while self._running and rclpy.ok(): + ret, frame = cap.read() + if not ret: + self.get_logger().warn(f"[cam {idx}] Frame read failed, retrying...") + time.sleep(0.5) + continue + + try: + msg = self._bridge.cv2_to_imgmsg(frame, encoding="bgr8") + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = f"camera_{idx}" + pub.publish(msg) + except Exception as exc: + self.get_logger().warn(f"[cam {idx}] Publish error: {exc}") + + time.sleep(interval) + + cap.release() + self.get_logger().info(f"[cam {idx}] Stopped.") + + def destroy_node(self) -> None: + self._running = False + for t in self._threads: + t.join(timeout=2.0) + super().destroy_node() + + +def main(args: Optional[List[str]] = None) -> None: + rclpy.init(args=args) + node = None + try: + node = UsbCameraRelayNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/subsystems/navigation/yolo_ros_bt/config/yolo_params.yaml b/src/subsystems/navigation/yolo_ros_bt/config/yolo_params.yaml new file mode 100644 index 00000000..e1983169 --- /dev/null +++ b/src/subsystems/navigation/yolo_ros_bt/config/yolo_params.yaml @@ -0,0 +1,19 @@ +yolo_node: + ros__parameters: + # One entry per camera. Index in this list determines the output namespace: + # camera 0 → /yolo/cam0/detections, /yolo/cam0/target_found, etc. + # camera 1 → /yolo/cam1/detections, /yolo/cam1/target_found, etc. + image_topics: + - /zed/zed_node/rgb/color/rect/image # ZED (camera 0) + - /cameras/cam0/image_raw # USB cam 0 + - /cameras/cam1/image_raw # USB cam 1 + - /cameras/cam2/image_raw # USB cam 2 + + target_classes: + - Bottle + - Mallet + - Rock-Pick-Hammer + + conf_thres: 0.5 + publish_annotated_image: true + queue_size: 5 diff --git a/src/subsystems/navigation/yolo_ros_bt/launch/yolo.ros.launch.py b/src/subsystems/navigation/yolo_ros_bt/launch/yolo.ros.launch.py index c9aeb15e..1872458d 100644 --- a/src/subsystems/navigation/yolo_ros_bt/launch/yolo.ros.launch.py +++ b/src/subsystems/navigation/yolo_ros_bt/launch/yolo.ros.launch.py @@ -1,17 +1,29 @@ from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + def generate_launch_description(): + default_params = os.path.join( + get_package_share_directory('yolo_ros_bt'), 'config', 'yolo_params.yaml' + ) + + params_file = LaunchConfiguration('params_file') + return LaunchDescription([ + DeclareLaunchArgument( + 'params_file', + default_value=default_params, + description='Full path to the YOLO params YAML', + ), Node( package='yolo_ros_bt', executable='yolo_node', name='yolo_node', output='screen', - parameters=[{ - 'image_topic': '/zed/zed_node/rgb/color/rect/image', - 'target_classes': ['Bottle', 'Mallet', 'Rock-Pick-Hammer'], - 'conf_thres': 0.5, - }] - ) - ]) \ No newline at end of file + parameters=[params_file], + ), + ]) diff --git a/src/subsystems/navigation/yolo_ros_bt/setup.py b/src/subsystems/navigation/yolo_ros_bt/setup.py index 20e5a857..25ab023f 100644 --- a/src/subsystems/navigation/yolo_ros_bt/setup.py +++ b/src/subsystems/navigation/yolo_ros_bt/setup.py @@ -25,6 +25,10 @@ os.path.join('share', package_name, 'models'), glob('models/*') ), + ( + os.path.join('share', package_name, 'config'), + glob('config/*.yaml') + ), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/subsystems/navigation/yolo_ros_bt/yolo_ros_bt/yolo_ros_node.py b/src/subsystems/navigation/yolo_ros_bt/yolo_ros_bt/yolo_ros_node.py index 3cf89cbd..3d0907a9 100644 --- a/src/subsystems/navigation/yolo_ros_bt/yolo_ros_bt/yolo_ros_node.py +++ b/src/subsystems/navigation/yolo_ros_bt/yolo_ros_bt/yolo_ros_node.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -from typing import Optional, List +from typing import Optional, List, NamedTuple import os import cv2 @@ -22,6 +22,14 @@ YOLO = None +class _CamPubs(NamedTuple): + detections: object + target_found: object + target_label: object + target_center: object + annotated_image: object + + class YoloRosNode(Node): def __init__(self) -> None: super().__init__('yolo_node') @@ -31,13 +39,12 @@ def __init__(self) -> None: self.declare_parameter('conf_thres', 0.5) self.declare_parameter('model_path', default_model_path) - self.declare_parameter('image_topic', '/camera/image_raw') + self.declare_parameter('image_topics', ['/camera/image_raw']) self.declare_parameter( 'target_classes', ['Bottle', 'Mallet', 'Rock-Pick-Hammer'] ) self.declare_parameter('publish_annotated_image', True) - self.declare_parameter('annotated_image_topic', '/yolo/annotated_image') self.declare_parameter('queue_size', 5) self.conf_thres: float = float( @@ -46,20 +53,10 @@ def __init__(self) -> None: self.model_path: str = ( self.get_parameter('model_path').get_parameter_value().string_value ) - self.image_topic: str = ( - self.get_parameter('image_topic').get_parameter_value().string_value - ) + image_topics: List[str] = list(self.get_parameter('image_topics').value) self.target_classes = self.get_parameter('target_classes').value - self.publish_annotated_image: bool = ( - self.get_parameter('publish_annotated_image') - .get_parameter_value() - .bool_value - ) - self.annotated_image_topic: str = ( - self.get_parameter('annotated_image_topic') - .get_parameter_value() - .string_value + self.get_parameter('publish_annotated_image').get_parameter_value().bool_value ) self.queue_size: int = int( self.get_parameter('queue_size').get_parameter_value().integer_value @@ -81,50 +78,33 @@ def __init__(self) -> None: self.get_logger().error(f'Failed to load YOLO model: {exc}') raise - self.image_sub = self.create_subscription( - Image, - self.image_topic, - self.image_callback, - self.queue_size - ) - - self.detections_pub = self.create_publisher( - Detection2DArray, - '/yolo/detections', - self.queue_size - ) - - self.target_found_pub = self.create_publisher( - Bool, - '/yolo/target_found', - self.queue_size - ) - - self.target_label_pub = self.create_publisher( - String, - '/yolo/target_label', - self.queue_size - ) - - self.target_center_pub = self.create_publisher( - PointStamped, - '/yolo/target_center', - self.queue_size - ) - - self.annotated_image_pub = self.create_publisher( - Image, - self.annotated_image_topic, - self.queue_size - ) + for idx, topic in enumerate(image_topics): + ns = f'/yolo/cam{idx}' + pubs = _CamPubs( + detections=self.create_publisher( + Detection2DArray, f'{ns}/detections', self.queue_size), + target_found=self.create_publisher( + Bool, f'{ns}/target_found', self.queue_size), + target_label=self.create_publisher( + String, f'{ns}/target_label', self.queue_size), + target_center=self.create_publisher( + PointStamped, f'{ns}/target_center', self.queue_size), + annotated_image=self.create_publisher( + Image, f'{ns}/annotated_image', self.queue_size), + ) + self.create_subscription( + Image, + topic, + lambda msg, p=pubs: self._image_callback(msg, p), + self.queue_size, + ) + self.get_logger().info(f'Camera {idx}: {topic} → {ns}/') self.get_logger().info('YOLO ROS node initialized') - self.get_logger().info(f'Subscribing to image topic: {self.image_topic}') self.get_logger().info(f'Target classes: {self.target_classes}') self.get_logger().info(f'Confidence threshold: {self.conf_thres:.2f}') - self.get_logger().info('Publishing detections to /yolo/detections') - def image_callback(self, msg: Image) -> None: + def _image_callback(self, msg: Image, pubs: _CamPubs) -> None: try: frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') except Exception as exc: @@ -139,16 +119,14 @@ def image_callback(self, msg: Image) -> None: detection_array = Detection2DArray() detection_array.header = msg.header - - target_found = False - best_target_conf = -1.0 - best_target_center = None - best_target_label = '' - annotated_frame = frame.copy() + img_h, img_w = frame.shape[:2] + norm_w = float(img_w) if img_w else 1.0 + norm_h = float(img_h) if img_h else 1.0 + if not results: - self.publish_empty_outputs(msg.header) + self._publish_empty(msg.header, pubs) return result = results[0] @@ -156,17 +134,17 @@ def image_callback(self, msg: Image) -> None: names = getattr(result, 'names', {}) if boxes is None or len(boxes) == 0: - self.detections_pub.publish(detection_array) - self.publish_target_outputs( - msg.header, - found=False, - target_center=None, - target_label='' - ) + pubs.detections.publish(detection_array) + self._publish_target(msg.header, pubs, found=False, center=None, label='') if self.publish_annotated_image: - self.publish_annotated(annotated_frame, msg.header) + self._publish_annotated(annotated_frame, msg.header, pubs) return + target_found = False + best_conf = -1.0 + best_center = None + best_label = '' + for box in boxes: conf = float(box.conf[0].item()) class_id = int(box.cls[0].item()) @@ -179,15 +157,15 @@ def image_callback(self, msg: Image) -> None: x_min, y_min, x_max, y_max = box.xyxy[0].tolist() center_x = (x_min + x_max) / 2.0 center_y = (y_min + y_max) / 2.0 - width = x_max - x_min - height = y_max - y_min + # Publish bbox in normalized [0, 1] image coordinates so the GUI + # can overlay it on any WebRTC stream regardless of encode size. detection_msg = Detection2D() detection_msg.header = msg.header - detection_msg.bbox.center.position.x = center_x - detection_msg.bbox.center.position.y = center_y - detection_msg.bbox.size_x = width - detection_msg.bbox.size_y = height + detection_msg.bbox.center.position.x = center_x / norm_w + detection_msg.bbox.center.position.y = center_y / norm_h + detection_msg.bbox.size_x = (x_max - x_min) / norm_w + detection_msg.bbox.size_y = (y_max - y_min) / norm_h hypothesis = ObjectHypothesisWithPose() hypothesis.hypothesis.class_id = class_name @@ -196,18 +174,18 @@ def image_callback(self, msg: Image) -> None: detection_array.detections.append(detection_msg) - if class_name in self.target_classes and conf > best_target_conf: + if class_name in self.target_classes and conf > best_conf: target_found = True - best_target_conf = conf - best_target_center = (center_x, center_y) - best_target_label = class_name + best_conf = conf + best_center = (center_x / norm_w, center_y / norm_h) + best_label = class_name cv2.rectangle( annotated_frame, (int(x_min), int(y_min)), (int(x_max), int(y_max)), (0, 255, 0), - 2 + 2, ) cv2.putText( annotated_frame, @@ -216,61 +194,54 @@ def image_callback(self, msg: Image) -> None: cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), - 2 + 2, ) - self.detections_pub.publish(detection_array) - - self.publish_target_outputs( - msg.header, + pubs.detections.publish(detection_array) + self._publish_target( + msg.header, pubs, found=target_found, - target_center=best_target_center, - target_label=best_target_label if target_found else '' + center=best_center, + label=best_label if target_found else '', ) - if self.publish_annotated_image: - self.publish_annotated(annotated_frame, msg.header) - - def publish_empty_outputs(self, header) -> None: - empty_array = Detection2DArray() - empty_array.header = header - self.detections_pub.publish(empty_array) - - self.publish_target_outputs( - header, - found=False, - target_center=None, - target_label='' - ) + self._publish_annotated(annotated_frame, msg.header, pubs) + + def _publish_empty(self, header, pubs: _CamPubs) -> None: + empty = Detection2DArray() + empty.header = header + pubs.detections.publish(empty) + self._publish_target(header, pubs, found=False, center=None, label='') - def publish_target_outputs( + def _publish_target( self, header, + pubs: _CamPubs, found: bool, - target_center: Optional[tuple], - target_label: str + center: Optional[tuple], + label: str, ) -> None: found_msg = Bool() found_msg.data = found - self.target_found_pub.publish(found_msg) + pubs.target_found.publish(found_msg) label_msg = String() - label_msg.data = target_label - self.target_label_pub.publish(label_msg) + label_msg.data = label + pubs.target_label.publish(label_msg) - if found and target_center is not None: + if found and center is not None: point_msg = PointStamped() point_msg.header = header - point_msg.point.x = float(target_center[0]) - point_msg.point.y = float(target_center[1]) + point_msg.point.x = float(center[0]) + point_msg.point.y = float(center[1]) point_msg.point.z = 0.0 - self.target_center_pub.publish(point_msg) + pubs.target_center.publish(point_msg) - def publish_annotated(self, frame, header) -> None: + def _publish_annotated(self, frame, header, pubs: _CamPubs) -> None: try: img_msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') img_msg.header = header - self.annotated_image_pub.publish(img_msg) + pubs.annotated_image.publish(img_msg) except Exception as exc: self.get_logger().warn(f'Failed to publish annotated image: {exc}') @@ -293,4 +264,4 @@ def main(args: Optional[List[str]] = None) -> None: if __name__ == '__main__': - main() \ No newline at end of file + main()