diff --git a/orbbec_camera/config/camera_params.yaml b/orbbec_camera/config/camera_params.yaml new file mode 100644 index 00000000..d455bec8 --- /dev/null +++ b/orbbec_camera/config/camera_params.yaml @@ -0,0 +1,45 @@ +# common params +depth_registration: false +enable_point_cloud: false +enable_colored_point_cloud: false +device_preset: "High Accuracy" +laser_on_off_mode: 1 # 0: off, 1: on-off, 1: off-on + +# When 3D reconstruction mode is enabled: +# - The laser will switch to on-off mode +# - IR images without the laser will be used for SLAM localization +# - Depth images with the laser will be used because they provide better depth quality +enable_3d_reconstruction_mode: true + +# color params +enable_color: true +color_width: 640 +color_height: 480 +color_fps: 60 +color_format: "YUYV" +enable_color_auto_exposure: false +color_exposure: 50 # 5ms +color_gain: -1 # -1 default + +# depth params +depth_width: 640 +depth_height: 480 +depth_fps: 60 +depth_format: "Y16" + +# ir exposure +enable_ir_auto_exposure: false +ir_exposure: 5000 # 5ms +ir_gain: 40 + +#left ir params +left_ir_width: 640 +left_ir_height: 480 +left_ir_fps: 60 +left_ir_format: "Y8" + +#right ir params +right_ir_width: 640 +right_ir_height: 480 +right_ir_fps: 60 +right_ir_format: "Y8" diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index 7d304fb1..f28ce21f 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -1,16 +1,48 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction 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.actions import PushRosNamespace, ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode -from launch_ros.actions import Node import os +import yaml + +def load_yaml(file_path): + with open(file_path, 'r') as f: + return yaml.safe_load(f) + +def merge_params(default_params, yaml_params): + for key, value in yaml_params.items(): + if key in default_params: + default_params[key] = value + return default_params + +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 + elif 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 = 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: convert_value(value) if key not in skip_convert else 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='true'), @@ -117,63 +149,54 @@ def generate_launch_description(): DeclareLaunchArgument('enable_laser', default_value='true'), DeclareLaunchArgument('depth_precision', default_value=''), DeclareLaunchArgument('device_preset', default_value='Default'), - # Laser on/off alternate mode, 0: off, 1: on-off alternate, 2: off-on alternate. DeclareLaunchArgument('laser_on_off_mode', default_value='0'), DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'), DeclareLaunchArgument('laser_energy_level', default_value='-1'), - # When 3D reconstruction mode is enabled: - # - The laser will switch to on-off mode - # - IR images without the laser will be used for SLAM localization - # - Depth images with the laser will be used because they provide better depth quality DeclareLaunchArgument('enable_3d_reconstruction_mode', default_value='false'), + 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="screen", ) ] - ) - # 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="screen", - ) - # Launch description - ld = LaunchDescription( - args - + [ - GroupAction( - [PushRosNamespace(LaunchConfiguration("camera_name")), container] - ) + else: + 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="screen", + ) + ]) ] - ) - return ld + + return LaunchDescription( + args + [ + OpaqueFunction(function=lambda context: create_node_action(context, args)) + ] + ) diff --git a/orbbec_camera/launch/multi_camera_synced.launch.py b/orbbec_camera/launch/multi_camera_synced.launch.py index 9eeba316..0b4452de 100644 --- a/orbbec_camera/launch/multi_camera_synced.launch.py +++ b/orbbec_camera/launch/multi_camera_synced.launch.py @@ -10,6 +10,8 @@ def generate_launch_description(): # Include launch files package_dir = get_package_share_directory('orbbec_camera') launch_file_dir = os.path.join(package_dir, 'launch') + config_file_dir = os.path.join(package_dir, 'config') + config_file_path = os.path.join(config_file_dir, 'camera_params.yaml') launch1_include = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_file_dir, 'gemini_330_series.launch.py') @@ -18,7 +20,8 @@ def generate_launch_description(): 'camera_name': 'front_camera', 'usb_port': '2-1.1', 'device_num': '4', - 'sync_mode': 'primary' + 'sync_mode': 'primary', + 'config_file': config_file_path, }.items() ) @@ -30,7 +33,8 @@ def generate_launch_description(): 'camera_name': 'left_camera', 'usb_port': '2-1.2.1', 'device_num': '4', - 'sync_mode': 'secondary_synced' + 'sync_mode': 'secondary_synced', + 'config_file': config_file_path, }.items() ) launch3_include = IncludeLaunchDescription( @@ -41,7 +45,8 @@ def generate_launch_description(): 'camera_name': 'right_camera', 'usb_port': '2-1.2.1', 'device_num': '4', - 'sync_mode': 'secondary_synced' + 'sync_mode': 'secondary_synced', + 'config_file': config_file_path, }.items() ) launch4_include = IncludeLaunchDescription( @@ -52,7 +57,8 @@ def generate_launch_description(): 'camera_name': 'right_camera', 'usb_port': '2-1.2.1', 'device_num': '4', - 'sync_mode': 'secondary_synced' + 'sync_mode': 'secondary_synced', + 'config_file': config_file_path, }.items() ) @@ -63,7 +69,7 @@ def generate_launch_description(): GroupAction([launch2_include]), GroupAction([launch3_include]), GroupAction([launch4_include]), - GroupAction([launch1_include]), + GroupAction([launch1_include]), # The primary camera should be launched at last ]) return ld