Feat/cam/comp options

setup_scripts/add_camera.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+"""Add a new camera to cameras_cpp/config: registers it in cameras.json and
+creates a params yaml file with its /dev/v4l/by-id device id.
+
+Usage:
+    python3 add_camera.py <name> <id>
+
+<id> can be given as:
+    - a full path, e.g. /dev/v4l/by-id/usb-046d_081d_374D8990-video-index0
+    - just the by-id fragment, e.g. usb-046d_081d_374D8990-video-index0
+The device id can be found by running: ls -l /dev/v4l/by-id/
+"""
+import argparse
+import json
+from pathlib import Path
+
+CONFIG_DIR = Path(__file__).resolve().parent.parent / "src" / "cameras_cpp" / "config"
+CAMERAS_JSON = CONFIG_DIR / "cameras.json"
+
+YAML_TEMPLATE = '''/**:
+  ros__parameters:
+    video_device: "{video_device}"
+    #image_width: 640
+    #image_height: 480
+    #framerate: 30.0
+    #pixel_format: "yuyv"
+    #image_raw:
+      #ffmpeg:
+        #encoder: "h264_nvmpi"
+        #bit_rate: 2000000
+        #gop_size: 20 # Set the Group of Pictures (GOP) size for the encoder. A smaller GOP size can improve error resilience but may increase file size, while a larger GOP size can reduce file size but may be less resilient to errors.
+        #qmax: 20 # Set the maximum quantization parameter (Q) for the encoder. Lower values result in higher quality but larger file sizes, while higher values result in lower quality but smaller file sizes.
+        #encoder_av_options: 'num_capture_buffers:4;profile:main;preset:ultrafast'
+'''
+
+
+def resolve_video_device(camera_id: str) -> str:
+    if camera_id.startswith("/dev/"):
+        return camera_id
+    return f"/dev/v4l/by-id/{camera_id}"
+
+
+def load_cameras_json() -> dict:
+    if CAMERAS_JSON.exists():
+        with open(CAMERAS_JSON, "r") as f:
+            return json.load(f)
+    return {"cameras": []}
+
+
+def add_camera(name: str, camera_id: str) -> None:
+    yaml_filename = f"{name}.yaml"
+    yaml_path = CONFIG_DIR / yaml_filename
+
+    data = load_cameras_json()
+    cameras = data.setdefault("cameras", [])
+    existing = next((c for c in cameras if c.get("name") == name), None)
+    if existing is not None:
+        existing["params_file"] = yaml_filename
+    else:
+        cameras.append({"name": name, "params_file": yaml_filename})
+
+    video_device = resolve_video_device(camera_id)
+
+    CONFIG_DIR.mkdir(parents=True, exist_ok=True)
+    with open(yaml_path, "w") as f:
+        f.write(YAML_TEMPLATE.format(video_device=video_device))
+
+    with open(CAMERAS_JSON, "w") as f:
+        json.dump(data, f, indent=4)
+        f.write("\n")
+
+    print(f"Added camera '{name}' -> {video_device}")
+    print(f"  Updated {CAMERAS_JSON}")
+    print(f"  Wrote   {yaml_path}")
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("name", help="Camera name, e.g. realsense415")
+    parser.add_argument(
+        "id",
+        help="by-id device fragment or full path, e.g. usb-046d_081d_374D8990-video-index0",
+    )
+    args = parser.parse_args()
+
+    add_camera(args.name, args.id)
+
+
+if __name__ == "__main__":
+    main()

src/cameras_cpp/config/cameras.json

@@ -1,28 +1,16 @@
 {
     "cameras": [
         {
-            "name": "realsense415",
-            "params_file": "realsense415.yaml"
+            "name": "arm_top",
+            "params_file": "arm_top.yaml"
         },
         {
-            "name": "realsense435i",
-            "params_file": "realsense435i.yaml"
+            "name": "front",
+            "params_file": "front.yaml"
         },
         {
-            "name": "logitech",
-            "params_file": "logitech.yaml"
-        },
-        {
-            "name": "suyin",
-            "params_file": "suyin.yaml"
-        },
-        {
-            "name": "science_webcam",
-            "params_file": "science_webcam.yaml"
-        },
-        {
-            "name": "science_fisheye",
-            "params_file": "science_fisheye.yaml"
+            "name": "pole",
+            "params_file": "pole.yaml"
         }
     ]
 }

src/cameras_cpp/launch/jetson_compressor.launch.py

@@ -2,8 +2,9 @@
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch_ros.actions import Node
-from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+from launch.substitutions import LaunchConfiguration
 
 DEFAULT_PARAMS = [
     {'image_raw.ffmpeg.encoder': 'hevc_nvmpi'},
@@ -16,43 +17,55 @@
 ]
 
 
-def generate_launch_description():
-    nodes = []
+def create_camera_node(camera_name, camera_params_path):
+    params = DEFAULT_PARAMS.copy()
+    params.append(camera_params_path)
+
+    return Node(
+        package='v4l2_camera',
+        executable='v4l2_camera_node',
+        name=camera_name,
+        output='screen',
+        parameters=params,
+        remappings=[
+            ('/image_raw', f'/{camera_name}/image_raw'),
+            ('/image_raw/ffmpeg', f'/{camera_name}/image_raw/ffmpeg'),
+        ],
+    )
+
+
+def choose_cameras(context, *args, **kwargs):
+    camera_args = LaunchConfiguration('cameras').perform(context)
+    camera_args_list = camera_args.split()
 
     cameras_config_path = os.path.join(
         get_package_share_directory('cameras_cpp'),
         'config', 'cameras.json'
     )
     with open(cameras_config_path, 'r') as f:
         cameras_json = json.load(f)
+    all_cameras = cameras_json.get('cameras', [])
 
-    cameras = cameras_json.get('cameras', [])
+    if len(camera_args_list) == 0:
+        cameras = all_cameras
+    else:
+        cameras = [c for c in all_cameras if c['name'] in camera_args_list]
 
+    nodes = []
     for camera in cameras:
         camera_name = camera['name']
         camera_params_file = camera['params_file']
         camera_params_path = os.path.join(
             get_package_share_directory('cameras_cpp'),
             'config', camera_params_file
         )
-        camera_node = create_camera_node(camera_name, camera_params_path)
-        nodes.append(camera_node)
+        nodes.append(create_camera_node(camera_name, camera_params_path))
 
-    return LaunchDescription(nodes)
+    return nodes
 
-def create_camera_node(camera_name, camera_params):
-
-    params = DEFAULT_PARAMS.copy()
-    params.append(camera_params)
 
-    return Node(
-        package='v4l2_camera',
-        executable='v4l2_camera_node',
-        name=camera_name,
-        output='screen',
-        parameters=params,
-        remappings=[
-            ('/image_raw', f'/{camera_name}/image_raw'),
-            ('/image_raw/ffmpeg', f'/{camera_name}/image_raw/ffmpeg'),
-        ],
-    )
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument('cameras', default_value=''),
+        OpaqueFunction(function=choose_cameras),
+    ])

src/cameras_cpp/launch/jetson_decompressor.launch.py

@@ -2,38 +2,56 @@
 import json
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
 
-def generate_launch_description():
-    broadcastor_nodes = []
+
+def create_broadcaster_node(camera_name):
+    return Node(
+        package='image_transport',
+        executable='republish',
+        name=f'{camera_name}_decoder_node',
+        remappings=[
+            ('in/ffmpeg', f'/{camera_name}/image_raw/ffmpeg'),
+            ('out', f'/{camera_name}/image_raw_decoded'),
+        ],
+        parameters=[
+            {'in.ffmpeg.decoders.hevc': 'hevc'},
+            {'in.ffmpeg.decoders.h264': 'h264'},
+        ],
+        output='screen',
+        arguments=['ffmpeg', 'raw']
+    )
+
+
+def choose_cameras(context, *args, **kwargs):
+    camera_args = LaunchConfiguration('cameras').perform(context)
+    camera_args_list = camera_args.split()
 
     cameras_config_path = os.path.join(
         get_package_share_directory('cameras_cpp'),
         'config', 'cameras.json'
     )
-
     with open(cameras_config_path, 'r') as f:
         cameras_json = json.load(f)
+    all_cameras = cameras_json.get('cameras', [])
 
-    cameras = cameras_json.get('cameras', [])
+    if len(camera_args_list) == 0:
+        cameras = all_cameras
+    else:
+        cameras = [c for c in all_cameras if c['name'] in camera_args_list]
 
+    nodes = []
     for camera in cameras:
         camera_name = camera['name']
-        broadcastor_node = Node(
-            package='image_transport',
-            executable='republish',
-            name=f'{camera_name}_decoder_node',
-            remappings=[
-                ('in/ffmpeg', f'/{camera_name}/image_raw/ffmpeg'),
-                ('out', f'/{camera_name}/image_raw_decoded'),
-            ],
-            parameters=[
-                {'in.ffmpeg.decoders.hevc': 'hevc'},
-                {'in.ffmpeg.decoders.h264': 'h264'},
-            ],
-            output='screen',
-            arguments=['ffmpeg', 'raw']
-        )
-        broadcastor_nodes.append(broadcastor_node)
-
-    return LaunchDescription(broadcastor_nodes)
+        nodes.append(create_broadcaster_node(camera_name))
+
+    return nodes
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument('cameras', default_value=''),
+        OpaqueFunction(function=choose_cameras),
+    ])