diff --git a/setup_scripts/add_camera.py b/setup_scripts/add_camera.py new file mode 100644 index 00000000..0cac9fb2 --- /dev/null +++ b/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 + + 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() diff --git a/src/cameras_cpp/config/realsense415.yaml b/src/cameras_cpp/config/arm_top.yaml similarity index 100% rename from src/cameras_cpp/config/realsense415.yaml rename to src/cameras_cpp/config/arm_top.yaml diff --git a/src/cameras_cpp/config/cameras.json b/src/cameras_cpp/config/cameras.json index f119fa13..e4789489 100644 --- a/src/cameras_cpp/config/cameras.json +++ b/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" } ] } \ No newline at end of file diff --git a/src/cameras_cpp/config/realsense435i.yaml b/src/cameras_cpp/config/front.yaml similarity index 100% rename from src/cameras_cpp/config/realsense435i.yaml rename to src/cameras_cpp/config/front.yaml diff --git a/src/cameras_cpp/config/logitech.yaml b/src/cameras_cpp/config/logitech.yaml deleted file mode 100644 index 16ab3f6e..00000000 --- a/src/cameras_cpp/config/logitech.yaml +++ /dev/null @@ -1,14 +0,0 @@ -/**: - ros__parameters: - video_device: "/dev/v4l/by-id/usb-046d_081d_374D8990-video-index0" - #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: 'preset:ultrafast,profile:baseline' \ No newline at end of file diff --git a/src/cameras_cpp/config/suyin.yaml b/src/cameras_cpp/config/pole.yaml similarity index 100% rename from src/cameras_cpp/config/suyin.yaml rename to src/cameras_cpp/config/pole.yaml diff --git a/src/cameras_cpp/config/science_fisheye.yaml b/src/cameras_cpp/config/science_fisheye.yaml deleted file mode 100644 index a96c0e5f..00000000 --- a/src/cameras_cpp/config/science_fisheye.yaml +++ /dev/null @@ -1,14 +0,0 @@ -/**: - ros__parameters: - video_device: "/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0" - #image_width: 640 - #image_height: 480 - #framerate: 30.0 - #pixel_format: "yuyv" - #image_raw: - #ffmpeg: - #encoder: "h264_nvmpi" - #bit_rate: 2000000 # Set the bit rate for the encoder. A higher bit rate results in better video quality but larger file sizes, while a lower bit rate results in lower video quality but smaller file sizes. - #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' diff --git a/src/cameras_cpp/config/science_webcam.yaml b/src/cameras_cpp/config/science_webcam.yaml deleted file mode 100644 index 89286099..00000000 --- a/src/cameras_cpp/config/science_webcam.yaml +++ /dev/null @@ -1,14 +0,0 @@ -/**: - ros__parameters: - video_device: "/dev/v4l/by-id/usb-046d_C270_HD_WEBCAM_C03D6F00-video-index0" - #image_width: 640 - #image_height: 480 - #framerate: 30.0 - #pixel_format: "yuyv" - #image_raw: - #ffmpeg: - #encoder: "h264_nvmpi" - #bit_rate: 2000000 # Set the bit rate for the encoder. A higher bit rate results in better video quality but larger file sizes, while a lower bit rate results in lower video quality but smaller file sizes. - #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' diff --git a/src/cameras_cpp/launch/jetson_compressor.launch.py b/src/cameras_cpp/launch/jetson_compressor.launch.py index e01af75c..9dfc4719 100644 --- a/src/cameras_cpp/launch/jetson_compressor.launch.py +++ b/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,8 +17,26 @@ ] -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'), @@ -25,9 +44,14 @@ def generate_launch_description(): ) 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'] @@ -35,24 +59,13 @@ def generate_launch_description(): 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'), - ], - ) \ No newline at end of file +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('cameras', default_value=''), + OpaqueFunction(function=choose_cameras), + ]) diff --git a/src/cameras_cpp/launch/jetson_decompressor.launch.py b/src/cameras_cpp/launch/jetson_decompressor.launch.py index 0f90553a..147799f4 100644 --- a/src/cameras_cpp/launch/jetson_decompressor.launch.py +++ b/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) \ No newline at end of file + nodes.append(create_broadcaster_node(camera_name)) + + return nodes + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('cameras', default_value=''), + OpaqueFunction(function=choose_cameras), + ]) \ No newline at end of file