feat(camera): discover and publish multiple capture devices

2026-10-20 10:05:58 +09:00
parent 7e76921c5e
commit 87b452035b
6 changed files with 199 additions and 14 deletions
@@ -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()