feat(camera): discover and publish multiple capture devices

README.md

@@ -9,14 +9,14 @@ The template folder remains `[lms-nav2-camera-test]nav2-camera-test_v1.0.0_ws`.
 - Package: `camera_test_pkg`
 - Launch: `camera.launch.py`
 - Viewer topic: `/camera_test/image_jpeg` (`sensor_msgs/msg/CompressedImage`, sensor-data QoS)
-- Only the selected `/dev/videoN` device is passed into the container.
+- Only selected capture devices are passed into the container, never the whole `/dev` directory.
 - No vehicle commands, fabricated calibration, or fixed camera mounting TF are published.
 
 ## Parameters
 
 | Parameter | Default | Meaning |
 | --- | --- | --- |
-| `video_device` | `/dev/video0` | V4L2 camera device |
+| `video_device` | `/dev/video0` | One V4L2 device, or `/dev` to discover up to eight capture endpoints |
 | `width` / `height` | `640` / `480` | Capture resolution supported by the camera |
 | `fps` | `30` | Capture frame rate |
 | `publish_fps` | `10` | Maximum publication rate |
@@ -25,11 +25,45 @@ The template folder remains `[lms-nav2-camera-test]nav2-camera-test_v1.0.0_ws`.
 | `jpeg_topic` | `/camera_test/image_jpeg` | Keep the default for the LMS viewer |
 | `frame_id` | `camera_test_link` | Image frame identifier |
 | `publish_raw` | `false` | Also publish `/camera_test/image_raw` |
+| `raw_topic` | `/camera_test/image_raw` | Raw topic for single-camera operation |
 
 `auto` negotiates raw/JPEG encoding, not resolution or frame rate. Use a mode
 reported by `v4l2-ctl --device /dev/video0 --list-formats-ext` when defaults are unsupported.
 This module provides a video preview, not calibrated visual SLAM input.
 
+## Multiple Cameras
+
+Set `video_device:=/dev` to inspect `/dev/video*` when the module starts.
+Capture-capable nodes are selected using V4L2 device capabilities; metadata-only,
+codec, non-character and inaccessible nodes are skipped. Device numbers are not
+physical-camera counts: a device can expose multiple usable capture streams.
+Discovery does not start streams on the host. Only the selected device nodes
+and Jetson VI channels are passed to Docker.
+
+Each endpoint has its own process and topics, for example
+`/camera_test/cameras/video0/image_jpeg` and
+`/camera_test/cameras/video2/image_jpeg`. Raw topics and frame IDs are also unique.
+In automatic mode these per-device names replace `jpeg_topic`, `raw_topic` and
+`frame_id`. Single-device operation retains the original names and overrides.
+The updated LMS viewer displays only endpoints actually publishing JPEG frames,
+with device labels and scrolling when needed. A failed camera does not stop
+other camera processes. Restart the module after adding, removing or reconnecting
+a camera so device discovery and failed capture processes are refreshed.
+
+Update and rebuild this module on Jetson before using automatic discovery.
+Old ARM64 prebuilt build/install archives must be regenerated before publishing.
+The runtime image is unchanged. Update the LMS FE as well; the remote viewer
+bridge receives the bundled bridge source on visualization startup.
+
+```sh
+./build.sh
+ROS_DOMAIN_ID=77 ./run.sh video_device:=/dev publish_fps:=5
+```
+
+All selected cameras use the same resolution, capture FPS and pixel format.
+Choose a mode supported by every camera, and reduce FPS/resolution if USB
+bandwidth, encoding CPU usage or remote network traffic becomes excessive.
+
 ## LMS Installation
 
 The environment profile supplies the runtime image artifact; the module supplies

run.sh

@@ -11,11 +11,21 @@ DEVICE=/dev/video0
 for arg in "$@"; do
   case "$arg" in video_device:=*) DEVICE=${arg#video_device:=} ;; esac
 done
-[[ "$DEVICE" =~ ^/dev/video[0-9]+$ ]] && [ -c "$DEVICE" ] || { echo "Camera device not found: $DEVICE" >&2; exit 1; }
+devices=()
+if [ "$DEVICE" = /dev ]; then
+  discovered=$(python3 "$MODULE_PATH/src/camera_test_pkg/camera_test_pkg/devices.py" /dev) || exit 1
+  mapfile -t devices <<< "$discovered"
+else
+  devices=("$DEVICE")
+fi
+for device in "${devices[@]}"; do
+  [[ "$device" =~ ^/dev/video[0-9]+$ ]] && [ -c "$device" ] || { echo "Camera device not found: $device" >&2; exit 1; }
+done
 test -f "$MODULE_PATH/install/setup.bash" || { echo 'Module is not built' >&2; exit 1; }
 
 docker rm -f "$CONTAINER_NAME" >/dev/null 2>&1 || true
-device_args=(--device "$DEVICE:$DEVICE")
+device_args=()
+for device in "${devices[@]}"; do device_args+=(--device "$device:$device"); done
 # Jetson VI capture opens additional channel devices from the container.
 for capture_device in /dev/capture-vi-channel*; do
   if [ -c "$capture_device" ]; then

src/camera_test_pkg/camera_test_pkg/devices.py

@@ -0,0 +1,53 @@
+"""Enumerate capture endpoints without opening a video stream."""
+import fcntl
+import glob
+import os
+import re
+import stat
+import struct
+import sys
+import warnings
+
+
+def capture_capabilities(data):
+    capabilities, device_caps = struct.unpack_from('II', data, 84)
+    caps = device_caps if capabilities & 0x80000000 else capabilities
+    return bool(caps & (0x00000001 | 0x00001000)) and not bool(
+        caps & (0x00008000 | 0x00004000)) and bool(
+        caps & (0x04000000 | 0x01000000))
+
+
+def discover_devices(directory='/dev', max_cameras=8):
+    if directory != '/dev' or not 1 <= max_cameras <= 8:
+        raise ValueError('Auto discovery requires /dev and max_cameras between 1 and 8')
+    devices = []
+    candidates = sorted(glob.glob('/dev/video*'), key=lambda p: (
+        int(p[10:]) if re.fullmatch(r'/dev/video[0-9]+', p) else sys.maxsize))
+    for path in candidates:
+        if not re.fullmatch(r'/dev/video[0-9]+', path):
+            continue
+        try:
+            if not stat.S_ISCHR(os.lstat(path).st_mode):
+                continue
+            fd = os.open(path, os.O_RDONLY | os.O_NONBLOCK | os.O_CLOEXEC)
+            try:
+                data = bytearray(104)
+                fcntl.ioctl(fd, 0x80685600, data, True)  # VIDIOC_QUERYCAP
+                if capture_capabilities(data):
+                    devices.append(path)
+            finally:
+                os.close(fd)
+        except OSError as error:
+            warnings.warn(f'Skipping {path}: {error}')
+    if len(devices) > max_cameras:
+        warnings.warn(f'Using first {max_cameras} of {len(devices)} capture endpoints')
+    if not devices:
+        raise RuntimeError('No usable video capture devices found in /dev; check camera connection and video group permissions')
+    return devices[:max_cameras]
+
+
+if __name__ == '__main__':
+    try:
+        print('\n'.join(discover_devices(sys.argv[1] if len(sys.argv) > 1 else '/dev')))
+    except (ValueError, RuntimeError) as error:
+        sys.exit(str(error))

src/camera_test_pkg/camera_test_pkg/gstreamer_camera.py

@@ -40,6 +40,7 @@ class GstreamerCameraTest(Node):
             'fps': 30, 'publish_fps': 10, 'pixel_format': 'auto',
             'frame_id': 'camera_test_link', 'jpeg_width': 640,
             'jpeg_topic': '/camera_test/image_jpeg', 'publish_raw': False,
+            'raw_topic': '/camera_test/image_raw',
         }
         for key, value in defaults.items():
             self.declare_parameter(key, value)
@@ -51,7 +52,7 @@ class GstreamerCameraTest(Node):
         self.jpeg_publisher = self.create_publisher(
             CompressedImage, p['jpeg_topic'], qos_profile_sensor_data)
         self.raw_publisher = self.create_publisher(
-            Image, '/camera_test/image_raw', qos_profile_sensor_data) if p['publish_raw'] else None
+            Image, p['raw_topic'], qos_profile_sensor_data) if p['publish_raw'] else None
         Gst.init(None)
         self.pipeline = Gst.parse_launch(capture_pipeline(
             p['video_device'], p['width'], p['height'], p['fps'], p['pixel_format']))

src/camera_test_pkg/launch/camera.launch.py

@@ -1,12 +1,12 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
+from camera_test_pkg.devices import discover_devices
 
 
-def generate_launch_description():
-    defaults = {
+DEFAULTS = {
         'video_device': ('/dev/video0', str),
         'width': ('640', int), 'height': ('480', int),
         'fps': ('30', int), 'publish_fps': ('10', int),
@@ -14,13 +14,33 @@ def generate_launch_description():
         'frame_id': ('camera_test_link', str),
         'jpeg_topic': ('/camera_test/image_jpeg', str),
         'publish_raw': ('false', bool),
+        'raw_topic': ('/camera_test/image_raw', str),
     }
+
+
+def camera_nodes(context):
+    device = LaunchConfiguration('video_device').perform(context)
+    automatic = device == '/dev'
+    devices = discover_devices() if automatic else [device]
+    nodes = []
+    for path in devices:
+        parameters = {key: ParameterValue(LaunchConfiguration(key), value_type=kind)
+                      for key, (_, kind) in DEFAULTS.items()}
+        parameters['video_device'] = path
+        name = path.rsplit('/', 1)[-1]
+        if automatic:
+            parameters.update(jpeg_topic=f'/camera_test/cameras/{name}/image_jpeg',
+                              raw_topic=f'/camera_test/cameras/{name}/image_raw',
+                              frame_id=f'camera_test_{name}_link')
+        nodes.append(Node(package='camera_test_pkg', executable='gstreamer_camera_test',
+                          name=f'camera_test_{name}' if automatic else 'camera_test',
+                          output='screen', parameters=[parameters]))
+    return nodes
+
+
+def generate_launch_description():
     return LaunchDescription([
         *[DeclareLaunchArgument(key, default_value=value)
-          for key, (value, _) in defaults.items()],
-        Node(package='camera_test_pkg', executable='gstreamer_camera_test',
-             name='camera_test', output='screen', parameters=[{
-                 key: ParameterValue(LaunchConfiguration(key), value_type=kind)
-                 for key, (_, kind) in defaults.items()
-             }]),
+          for key, (value, _) in DEFAULTS.items()],
+        OpaqueFunction(function=camera_nodes),
     ])

tests/test_devices.py

@@ -0,0 +1,67 @@
+import importlib.util
+from pathlib import Path
+import stat
+import struct
+from types import SimpleNamespace
+import unittest
+from unittest.mock import patch
+
+path = Path(__file__).parents[1] / 'src/camera_test_pkg/camera_test_pkg/devices.py'
+spec = importlib.util.spec_from_file_location('camera_devices', path)
+devices = importlib.util.module_from_spec(spec)
+spec.loader.exec_module(devices)
+
+
+def capability(caps, device_caps=0):
+    data = bytearray(104)
+    struct.pack_into('II', data, 84, caps, device_caps)
+    return data
+
+
+class DeviceTests(unittest.TestCase):
+    def test_capture_only_not_metadata_or_codec(self):
+        for caps in [0x04000001, 0x04001000, 0x01000001]:
+            self.assertTrue(devices.capture_capabilities(capability(caps)))
+        for caps in [0x04800000, 0x04008001, 0x04004001, 0x04000002, 1]:
+            self.assertFalse(devices.capture_capabilities(capability(caps)))
+        self.assertFalse(devices.capture_capabilities(capability(0x84000001, 0x04800000)))
+
+    def test_discovery_filters_nodes_and_sorts_numbers(self):
+        def query(fd, request, data, mutate):
+            self.assertEqual(request, 0x80685600)
+            data[:] = capability(0x04800000 if fd == 1 else 0x04000001)
+        with patch.object(devices.glob, 'glob', return_value=['/dev/video10', '/dev/video1', '/dev/video0', '/dev/videobad']), \
+             patch.object(devices.os, 'lstat', return_value=SimpleNamespace(st_mode=stat.S_IFCHR)), \
+             patch.object(devices.os, 'open', side_effect=lambda path, flags: int(path[10:])), \
+             patch.object(devices.os, 'close') as close, patch.object(devices.fcntl, 'ioctl', side_effect=query):
+            self.assertEqual(devices.discover_devices(), ['/dev/video0', '/dev/video10'])
+            self.assertEqual(close.call_count, 3)
+
+    def test_no_devices_and_invalid_roots_fail_without_scanning(self):
+        with patch.object(devices.glob, 'glob', return_value=[]):
+            with self.assertRaisesRegex(RuntimeError, 'No usable'):
+                devices.discover_devices()
+            with self.assertRaises(ValueError):
+                devices.discover_devices('/etc')
+
+    def test_launch_creates_independent_topic_and_raw_streams(self):
+        import ast
+        source = ast.parse((path.parents[1] / 'launch/camera.launch.py').read_text())
+        nodes = [node for node in source.body if not isinstance(node, (ast.Import, ast.ImportFrom))]
+        class Configuration:
+            def __init__(self, name): self.name = name
+            def perform(self, context): return context[self.name]
+        scope = {'LaunchConfiguration': Configuration, 'ParameterValue': lambda value, value_type: value,
+                 'Node': lambda **kwargs: kwargs, 'discover_devices': lambda: ['/dev/video0', '/dev/video2']}
+        exec(compile(ast.Module(body=nodes, type_ignores=[]), 'camera-launch', 'exec'), scope)
+        result = scope['camera_nodes']({'video_device': '/dev'})
+        self.assertEqual([node['name'] for node in result], ['camera_test_video0', 'camera_test_video2'])
+        self.assertEqual(result[1]['parameters'][0]['jpeg_topic'], '/camera_test/cameras/video2/image_jpeg')
+        self.assertEqual(result[1]['parameters'][0]['raw_topic'], '/camera_test/cameras/video2/image_raw')
+        single = scope['camera_nodes']({'video_device': '/dev/video0'})
+        self.assertEqual(single[0]['name'], 'camera_test')
+        self.assertEqual(single[0]['parameters'][0]['jpeg_topic'].name, 'jpeg_topic')
+
+
+if __name__ == '__main__':
+    unittest.main()