diff --git a/orbbec_camera/config/camera_335_params.yaml b/orbbec_camera/config/camera_335_params.yaml new file mode 100644 index 00000000..2471a708 --- /dev/null +++ b/orbbec_camera/config/camera_335_params.yaml @@ -0,0 +1,49 @@ +# common params +depth_registration: false +enable_d2c_viewer: false +enable_point_cloud: false +enable_colored_point_cloud: false +enable_ldp: false +device_preset: "Default" + + +# enable_laser: true +interleave_ae_mode: "laser" +interleave_frame_enable: true +interleave_skip_enable: false +interleave_skip_index: 1 +time_domain: "global" # global, device, system +enable_sync_host_time: true + +trigger_out_enabled: true +frames_per_trigger: 0 +software_trigger_period: 0 + + +# color params +enable_color: false +color_width: 640 +color_height: 480 +color_fps: 30 +color_format: "MJPG" +enable_color_auto_exposure: false +color_exposure: 50 # 5ms +color_gain: 16 # -1 default + + +#left ir params +enable_left_ir: true +left_ir_width: 640 +left_ir_height: 480 +left_ir_fps: 30 +left_ir_format: "Y8" +enable_ir_auto_exposure: true +ir_exposure: 5000 # 5ms +ir_gain: 16 # -1 default + +# depth params +enable_depth: true +depth_width: 640 +depth_height: 480 +depth_fps: 30 +depth_format: "Y16" diff --git a/orbbec_camera/launch/gemini_330_series_interleave_laser_g335_components.launch.py b/orbbec_camera/launch/gemini_330_series_interleave_laser_g335_components.launch.py new file mode 100644 index 00000000..94ae454e --- /dev/null +++ b/orbbec_camera/launch/gemini_330_series_interleave_laser_g335_components.launch.py @@ -0,0 +1,245 @@ +import os +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode +from launch.conditions import UnlessCondition +from launch_ros.actions import LoadComposableNodes + + + +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: (value if key in skip_convert else convert_value(value)) + for key, value in default_params.items() + } + + +def generate_launch_description(): + args = [ + DeclareLaunchArgument('camera_name', default_value='camera'), + DeclareLaunchArgument('depth_registration', default_value='true'), + DeclareLaunchArgument('serial_number', default_value=''), + DeclareLaunchArgument('usb_port', default_value=''), + DeclareLaunchArgument('device_num', default_value='1'), + DeclareLaunchArgument('point_cloud_qos', default_value='default'), + DeclareLaunchArgument('enable_point_cloud', default_value='false'), + DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), + DeclareLaunchArgument('cloud_frame_id', default_value=''), + DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_width', default_value='0'), + DeclareLaunchArgument('color_height', default_value='0'), + DeclareLaunchArgument('color_fps', default_value='30'), + DeclareLaunchArgument('color_format', default_value='MJPG'), + DeclareLaunchArgument('enable_color', default_value='true'), + DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_camera_info_qos', default_value='default'), + DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), + DeclareLaunchArgument('color_exposure', default_value='-1'), + DeclareLaunchArgument('color_gain', default_value='-1'), + DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'), + DeclareLaunchArgument('color_white_balance', default_value='-1'), + DeclareLaunchArgument('color_ae_max_exposure', default_value='-1'), + DeclareLaunchArgument('color_brightness', default_value='-1'), + DeclareLaunchArgument('color_ae_roi_left', default_value='-1'), + DeclareLaunchArgument('color_ae_roi_top', default_value='-1'), + DeclareLaunchArgument('color_ae_roi_right', default_value='-1'), + DeclareLaunchArgument('color_ae_roi_bottom', default_value='-1'), + DeclareLaunchArgument('depth_width', default_value='0'), + DeclareLaunchArgument('depth_height', default_value='0'), + DeclareLaunchArgument('depth_fps', default_value='30'), + DeclareLaunchArgument('depth_format', default_value='Y16'), + DeclareLaunchArgument('enable_depth', default_value='true'), + DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), + DeclareLaunchArgument('depth_ae_roi_left', default_value='-1'), + DeclareLaunchArgument('depth_ae_roi_top', default_value='-1'), + DeclareLaunchArgument('depth_ae_roi_right', default_value='-1'), + DeclareLaunchArgument('depth_ae_roi_bottom', default_value='-1'), + DeclareLaunchArgument('left_ir_width', default_value='0'), + DeclareLaunchArgument('left_ir_height', default_value='0'), + DeclareLaunchArgument('left_ir_fps', default_value='30'), + DeclareLaunchArgument('left_ir_format', default_value='Y8'), + DeclareLaunchArgument('enable_left_ir', default_value='true'), + DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), + DeclareLaunchArgument('right_ir_width', default_value='0'), + DeclareLaunchArgument('right_ir_height', default_value='0'), + DeclareLaunchArgument('right_ir_fps', default_value='30'), + DeclareLaunchArgument('right_ir_format', default_value='Y8'), + DeclareLaunchArgument('enable_right_ir', default_value='false'), + DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), + DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('ir_exposure', default_value='-1'), + DeclareLaunchArgument('ir_gain', default_value='-1'), + DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'), + DeclareLaunchArgument('ir_brightness', default_value='-1'), + DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), + DeclareLaunchArgument('enable_accel', default_value='false'), + DeclareLaunchArgument('accel_rate', default_value='200hz'), + DeclareLaunchArgument('accel_range', default_value='4g'), + DeclareLaunchArgument('enable_gyro', default_value='false'), + DeclareLaunchArgument('gyro_rate', default_value='200hz'), + DeclareLaunchArgument('gyro_range', default_value='1000dps'), + DeclareLaunchArgument('liner_accel_cov', default_value='0.01'), + DeclareLaunchArgument('angular_vel_cov', default_value='0.01'), + DeclareLaunchArgument('publish_tf', default_value='true'), + DeclareLaunchArgument('tf_publish_rate', default_value='0.0'), + DeclareLaunchArgument('ir_info_url', default_value=''), + DeclareLaunchArgument('color_info_url', default_value=''), + DeclareLaunchArgument('log_level', default_value='none'), + DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), + DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_ldp', default_value='true'), + DeclareLaunchArgument('enable_soft_filter', default_value='true'), + DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), + DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), + DeclareLaunchArgument('sync_mode', default_value='standalone'), + DeclareLaunchArgument('depth_delay_us', default_value='0'), + DeclareLaunchArgument('color_delay_us', default_value='0'), + DeclareLaunchArgument('trigger2image_delay_us', default_value='0'), + DeclareLaunchArgument('trigger_out_delay_us', default_value='0'), + DeclareLaunchArgument('trigger_out_enabled', default_value='true'), + DeclareLaunchArgument('frames_per_trigger', default_value='2'), + DeclareLaunchArgument('software_trigger_period', default_value='33'), # ms + DeclareLaunchArgument('enable_frame_sync', default_value='true'), + DeclareLaunchArgument('ordered_pc', default_value='false'), + DeclareLaunchArgument('use_hardware_time', default_value='true'), + DeclareLaunchArgument('enable_depth_scale', default_value='true'), + DeclareLaunchArgument('enable_decimation_filter', default_value='false'), + DeclareLaunchArgument('enable_hdr_merge', default_value='false'), + DeclareLaunchArgument('enable_sequence_id_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_hole_filling_filter', default_value='false'), + DeclareLaunchArgument('decimation_filter_scale_', default_value='-1'), + DeclareLaunchArgument('sequence_id_filter_id', 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='80'), + 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('hdr_merge_exposure_1', default_value='-1'), + DeclareLaunchArgument('hdr_merge_gain_1', default_value='-1'), + DeclareLaunchArgument('hdr_merge_exposure_2', default_value='-1'), + DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), + DeclareLaunchArgument('align_mode', default_value='SW'), + DeclareLaunchArgument('diagnostic_period', default_value='1.0'), + DeclareLaunchArgument('enable_laser', default_value='true'), + DeclareLaunchArgument('depth_precision', default_value=''), + DeclareLaunchArgument('device_preset', default_value='Default'), + DeclareLaunchArgument('laser_on_off_mode', default_value='0'), + DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'), + DeclareLaunchArgument('laser_energy_level', default_value='-1'), + DeclareLaunchArgument('enable_3d_reconstruction_mode', default_value='false'), + DeclareLaunchArgument('enable_sync_host_time', default_value='true'), + DeclareLaunchArgument('time_domain', default_value='device'), + DeclareLaunchArgument('enable_color_undistortion', default_value='false'), + DeclareLaunchArgument('config_file_path', default_value=''), + DeclareLaunchArgument('enable_heartbeat', default_value='false'), + DeclareLaunchArgument('enable_hardware_reset', default_value='false'), + DeclareLaunchArgument('interleave_ae_mode', default_value='laser'), # 'hdr' or 'laser' + DeclareLaunchArgument('interleave_frame_enable', default_value='true'), + DeclareLaunchArgument('interleave_skip_enable', default_value='true'), + DeclareLaunchArgument('interleave_skip_index', default_value='1'), # 0:partern ir 1:flood ir + DeclareLaunchArgument('use_intra_process_comms', default_value='false'), + DeclareLaunchArgument('attach_to_shared_component_container', default_value='false'), + DeclareLaunchArgument('component_container_name', default_value='orbbec_container'), + ] + + 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=params, + output="screen", + ) + ] + else: + attach_to_shared_component_container_arg = LaunchConfiguration('attach_to_shared_component_container', default=False) + component_container_name_arg = LaunchConfiguration('component_container_name', default='orbbec_container') + + orbbec_container = Node( + name=component_container_name_arg, + package='rclcpp_components', + executable='component_container_mt', + output='screen', + condition=UnlessCondition(attach_to_shared_component_container_arg) + ) + return [ + orbbec_container, + LoadComposableNodes( + target_container=component_container_name_arg, + composable_node_descriptions=[ + ComposableNode( + namespace=LaunchConfiguration("camera_name"), + name=LaunchConfiguration("camera_name"), + package='orbbec_camera', + plugin='orbbec_camera::OBCameraNodeDriver', + parameters=params, + extra_arguments=[{'use_intra_process_comms': LaunchConfiguration("use_intra_process_comms")}], + ) + ] + ) + ] + + return LaunchDescription( + args + [ + OpaqueFunction(function=lambda context: create_node_action(context, args)) + ] + ) diff --git a/orbbec_camera/launch/multi_camera_synced_components.launch.py b/orbbec_camera/launch/multi_camera_synced_components.launch.py new file mode 100644 index 00000000..9d67e080 --- /dev/null +++ b/orbbec_camera/launch/multi_camera_synced_components.launch.py @@ -0,0 +1,80 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, ExecuteProcess +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +from launch.conditions import UnlessCondition, IfCondition +from launch.substitutions import LaunchConfiguration +from launch.actions import TimerAction +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_335_path = os.path.join(config_file_dir, "camera_335_params.yaml") + config_file_335L_path = os.path.join(config_file_dir, "camera_335L_params.yaml") + + use_intra_process_comms_arg = DeclareLaunchArgument( + 'use_intra_process_comms', default_value='false', + ) + attach_to_shared_component_container_arg = DeclareLaunchArgument( + 'attach_to_shared_component_container', default_value='false', + ) + + shared_container_name = "shared_orbbec_container" + shared_container = Node( + name=shared_container_name, + package='rclcpp_components', + executable='component_container_mt', + output='screen', + condition=IfCondition(LaunchConfiguration('attach_to_shared_component_container')) + ) + + G0_6D = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335_components.launch.py") + ), + launch_arguments={ + "camera_name": "G0_6D", + "usb_port": "2-3.2", + "device_num": "2", + "sync_mode": "primary", + 'use_intra_process_comms': LaunchConfiguration("use_intra_process_comms"), + 'attach_to_shared_component_container': LaunchConfiguration("attach_to_shared_component_container"), + 'component_container_name': shared_container_name, + "config_file_path": config_file_335_path, + }.items(), + ) + + G1_6H = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335_components.launch.py") + ), + launch_arguments={ + "camera_name": "G1_6H", + "usb_port": "2-3.1", + "device_num": "2", + "sync_mode": "secondary_synced", + 'use_intra_process_comms': LaunchConfiguration("use_intra_process_comms"), + 'attach_to_shared_component_container': LaunchConfiguration("attach_to_shared_component_container"), + 'component_container_name': shared_container_name, + "config_file_path": config_file_335_path, + }.items(), + ) + + + # If you need more cameras, just add more launch_include here, and change the usb_port and device_num + + + + ld = LaunchDescription([ + use_intra_process_comms_arg, + attach_to_shared_component_container_arg, + shared_container, + GroupAction([G0_6D]), + TimerAction(period=0.5, actions=[GroupAction([G1_6H])]), + ]) + + return ld +