From 87b452035b3202e109a1f7222b0aeacc8c7f4557 Mon Sep 17 00:00:00 2001 From: chyoo Date: Tue, 20 Oct 2026 10:05:58 +0900 Subject: [PATCH] feat(camera): discover and publish multiple capture devices --- README.md | 38 ++++++++++- run.sh | 14 +++- .../camera_test_pkg/devices.py | 53 +++++++++++++++ .../camera_test_pkg/gstreamer_camera.py | 3 +- src/camera_test_pkg/launch/camera.launch.py | 38 ++++++++--- tests/test_devices.py | 67 +++++++++++++++++++ 6 files changed, 199 insertions(+), 14 deletions(-) create mode 100644 src/camera_test_pkg/camera_test_pkg/devices.py create mode 100644 tests/test_devices.py diff --git a/README.md b/README.md index 445c9bd..730eef3 100644 --- a/README.md +++ b/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 diff --git a/run.sh b/run.sh index 8639fba..584a7a6 100755 --- a/run.sh +++ b/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 diff --git a/src/camera_test_pkg/camera_test_pkg/devices.py b/src/camera_test_pkg/camera_test_pkg/devices.py new file mode 100644 index 0000000..cbb4bc7 --- /dev/null +++ b/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)) diff --git a/src/camera_test_pkg/camera_test_pkg/gstreamer_camera.py b/src/camera_test_pkg/camera_test_pkg/gstreamer_camera.py index 406eb36..1a835aa 100644 --- a/src/camera_test_pkg/camera_test_pkg/gstreamer_camera.py +++ b/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'])) diff --git a/src/camera_test_pkg/launch/camera.launch.py b/src/camera_test_pkg/launch/camera.launch.py index 49d6f60..b3f07e0 100644 --- a/src/camera_test_pkg/launch/camera.launch.py +++ b/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), ]) diff --git a/tests/test_devices.py b/tests/test_devices.py new file mode 100644 index 0000000..9b2605d --- /dev/null +++ b/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()