Usb camera to ros relay

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

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

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],
+        ),
+    ])

src/subsystems/navigation/usb_camera_relay/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>usb_camera_relay</name>
+  <version>0.0.0</version>
+  <description>Publishes USB camera streams as sensor_msgs/Image ROS topics</description>
+  <maintainer email="todo@todo.com">UMD Loop</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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

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

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()

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

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,
-            }]
-        )
-    ])
+            parameters=[params_file],
+        ),
+    ])

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,