From 28495a8f316da1e8730ba9a50e91af6fa54493a8 Mon Sep 17 00:00:00 2001 From: slz Date: Mon, 20 Apr 2026 13:55:32 +0800 Subject: [PATCH] feat: add YAML configuration for Gemini 2L dual IR mode --- orbbec_camera/config/gemini2L_dual_ir.yaml | 22 +++ orbbec_camera/launch/gemini2L.launch.py | 207 +++++++++++++-------- 2 files changed, 154 insertions(+), 75 deletions(-) create mode 100644 orbbec_camera/config/gemini2L_dual_ir.yaml diff --git a/orbbec_camera/config/gemini2L_dual_ir.yaml b/orbbec_camera/config/gemini2L_dual_ir.yaml new file mode 100644 index 00000000..68158f57 --- /dev/null +++ b/orbbec_camera/config/gemini2L_dual_ir.yaml @@ -0,0 +1,22 @@ +# Gemini 2L dual IR mode overrides. +# Usage: +# ros2 launch orbbec_camera gemini2L.launch.py config_file_path:=gemini2L_dual_ir.yaml + +depth_work_mode: In-scene Calibration +enable_ir: false + +enable_left_ir: true +left_ir_width: 1280 +left_ir_height: 800 +left_ir_fps: 30 +left_ir_format: Y8 +left_ir_qos: default +left_ir_camera_info_qos: default + +enable_right_ir: true +right_ir_width: 1280 +right_ir_height: 800 +right_ir_fps: 30 +right_ir_format: Y8 +right_ir_qos: default +right_ir_camera_info_qos: default diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index 67930366..65fb1645 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -1,16 +1,78 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import PushRosNamespace -from launch.actions import GroupAction -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch_ros.actions import Node import os +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, Node, PushRosNamespace +from launch_ros.descriptions import ComposableNode + + +def load_yaml(file_path): + try: + with open(file_path, "r") as f: + return yaml.safe_load(f) + except Exception as e: + print(f"[orbbec_camera] Failed to load yaml '{file_path}': {e}") + return {} + + +def merge_params(default_params, yaml_params): + if not isinstance(yaml_params, dict): + print("[orbbec_camera] YAML parameters should be a dictionary.") + return default_params + for key, value in yaml_params.items(): + default_params[key] = value + return default_params + + +def resolve_config_file_path(config_file_path: str) -> str: + if not config_file_path: + return config_file_path + if os.path.isabs(config_file_path) and os.path.exists(config_file_path): + return config_file_path + if os.path.exists(config_file_path): + return config_file_path + config_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), "..", "config")) + candidate = os.path.join(config_dir, config_file_path) + if os.path.exists(candidate): + return candidate + return config_file_path + + +def convert_value(value): + if isinstance(value, str): + try: + return int(value) + except ValueError: + pass + try: + return float(value) + except ValueError: + pass + if value.lower() == "true": + return True + if value.lower() == "false": + return False + return value + + +def load_parameters(context, args): + default_params = {arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args} + config_file_path = resolve_config_file_path( + LaunchConfiguration("config_file_path").perform(context) + ) + if config_file_path: + yaml_params = load_yaml(config_file_path) + default_params = merge_params(default_params, yaml_params) + skip_convert = {"config_file_path", "usb_port", "serial_number"} + + return { + key: (value if key in skip_convert else convert_value(value)) + for key, value in default_params.items() + } def generate_launch_description(): - # Declare arguments args = [ DeclareLaunchArgument("camera_name", default_value="camera"), DeclareLaunchArgument("depth_registration", default_value="false"), @@ -39,8 +101,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_color_auto_white_balance", default_value="true"), DeclareLaunchArgument("color_white_balance", default_value="-1"), DeclareLaunchArgument("color_brightness", default_value="-1"), - DeclareLaunchArgument('enable_color_decimation_filter', default_value='false'), - DeclareLaunchArgument('color_decimation_filter_scale', default_value='-1'), + DeclareLaunchArgument("enable_color_decimation_filter", default_value="false"), + DeclareLaunchArgument("color_decimation_filter_scale", default_value="-1"), DeclareLaunchArgument("depth_width", default_value="1280"), DeclareLaunchArgument("depth_height", default_value="800"), DeclareLaunchArgument("depth_fps", default_value="30"), @@ -79,32 +141,32 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), - DeclareLaunchArgument('disparity_to_depth_mode', default_value='SW'), + DeclareLaunchArgument("disparity_to_depth_mode", default_value="SW"), DeclareLaunchArgument("enable_ldp", default_value="true"), - DeclareLaunchArgument('enable_decimation_filter', default_value='false'), - DeclareLaunchArgument('enable_threshold_filter', default_value='false'), - DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'), - DeclareLaunchArgument('enable_spatial_filter', default_value='false'), - DeclareLaunchArgument('enable_temporal_filter', default_value='false'), - DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), - DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), - DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), - DeclareLaunchArgument('threshold_filter_max', default_value='-1'), - DeclareLaunchArgument('threshold_filter_min', default_value='-1'), - DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'), - DeclareLaunchArgument('noise_removal_filter_max_size', default_value='200'), - DeclareLaunchArgument('spatial_filter_alpha', default_value='-1.0'), - DeclareLaunchArgument('spatial_filter_diff_threshold', default_value='-1'), - DeclareLaunchArgument('spatial_filter_magnitude', default_value='-1'), - DeclareLaunchArgument('spatial_filter_radius', default_value='-1'), - DeclareLaunchArgument('temporal_filter_diff_threshold', default_value='-1.0'), - DeclareLaunchArgument('temporal_filter_weight', default_value='-1.0'), - DeclareLaunchArgument('hole_filling_filter_mode', default_value=''), + DeclareLaunchArgument("enable_decimation_filter", default_value="false"), + DeclareLaunchArgument("enable_threshold_filter", default_value="false"), + DeclareLaunchArgument("enable_noise_removal_filter", default_value="true"), + DeclareLaunchArgument("enable_spatial_filter", default_value="false"), + DeclareLaunchArgument("enable_temporal_filter", default_value="false"), + DeclareLaunchArgument("enable_disparity_to_depth", default_value="true"), + DeclareLaunchArgument("enable_hole_filling_filter", default_value="false"), + DeclareLaunchArgument("decimation_filter_scale", default_value="-1"), + DeclareLaunchArgument("threshold_filter_max", default_value="-1"), + DeclareLaunchArgument("threshold_filter_min", default_value="-1"), + DeclareLaunchArgument("noise_removal_filter_min_diff", default_value="256"), + DeclareLaunchArgument("noise_removal_filter_max_size", default_value="200"), + DeclareLaunchArgument("spatial_filter_alpha", default_value="-1.0"), + DeclareLaunchArgument("spatial_filter_diff_threshold", default_value="-1"), + DeclareLaunchArgument("spatial_filter_magnitude", default_value="-1"), + DeclareLaunchArgument("spatial_filter_radius", default_value="-1"), + DeclareLaunchArgument("temporal_filter_diff_threshold", default_value="-1.0"), + DeclareLaunchArgument("temporal_filter_weight", default_value="-1.0"), + DeclareLaunchArgument("hole_filling_filter_mode", default_value=""), # Depth work mode support is as follows: # Unbinned Dense Default # Unbinned Sparse Default # Binned Sparse Default - DeclareLaunchArgument("depth_work_mode", default_value=""), + DeclareLaunchArgument("depth_work_mode", default_value="Unbinned Dense Default"), DeclareLaunchArgument("sync_mode", default_value="standalone"), DeclareLaunchArgument("depth_delay_us", default_value="0"), DeclareLaunchArgument("color_delay_us", default_value="0"), @@ -120,56 +182,51 @@ def generate_launch_description(): DeclareLaunchArgument("enable_heartbeat", default_value="false"), DeclareLaunchArgument("enable_firmware_log", default_value="false"), DeclareLaunchArgument("time_domain", default_value="global"), - DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'), - DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), + DeclareLaunchArgument("enable_frame_timestamp_csv", default_value="false"), + DeclareLaunchArgument("frame_timestamp_csv_file", default_value=""), + DeclareLaunchArgument("config_file_path", default_value=""), ] - # Node configuration - parameters = [{arg.name: LaunchConfiguration(arg.name)} for arg in args] - # get ROS_DISTRO - ros_distro = os.environ["ROS_DISTRO"] - if ros_distro == "foxy": - return LaunchDescription( - args - + [ + def get_params(context, args): + return [load_parameters(context, args)] + + def create_node_action(context, args): + params = get_params(context, args) + ros_distro = os.environ.get("ROS_DISTRO", "humble") + if ros_distro == "foxy": + return [ Node( package="orbbec_camera", executable="orbbec_camera_node", name="ob_camera_node", namespace=LaunchConfiguration("camera_name"), - parameters=parameters, + parameters=params, output="log", ) ] - ) - # Define the ComposableNode - else: - # Define the ComposableNode - compose_node = ComposableNode( - package="orbbec_camera", - plugin="orbbec_camera::OBCameraNodeDriver", - name=LaunchConfiguration("camera_name"), - namespace="", - parameters=parameters, - ) - # Define the ComposableNodeContainer - container = ComposableNodeContainer( - name="camera_container", - namespace="", - package="rclcpp_components", - executable="component_container", - composable_node_descriptions=[ - compose_node, - ], - output="log", - ) - # Launch description - ld = LaunchDescription( - args - + [ - GroupAction( - [PushRosNamespace(LaunchConfiguration("camera_name")), container] - ) - ] - ) - return ld + return [ + GroupAction( + [ + PushRosNamespace(LaunchConfiguration("camera_name")), + ComposableNodeContainer( + name="camera_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="orbbec_camera", + plugin="orbbec_camera::OBCameraNodeDriver", + name=LaunchConfiguration("camera_name"), + parameters=params, + ), + ], + output="log", + ), + ] + ) + ] + + return LaunchDescription( + args + [OpaqueFunction(function=lambda context: create_node_action(context, args))] + )