From 282ab4b022198276aa94dc5e23f720684166a88a Mon Sep 17 00:00:00 2001 From: slz Date: Tue, 18 Aug 2026 15:13:52 +0800 Subject: [PATCH 1/6] fix: update color image layout on profile switch --- orbbec_camera/src/ob_camera_node.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 3663e70a..1aa56019 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -3933,6 +3933,12 @@ void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) { encoding_[stream_index] = is_depth_stream ? sensor_msgs::image_encodings::TYPE_16UC1 : sensor_msgs::image_encodings::MONO16; unit_step_size_[stream_index] = sizeof(uint16_t); + } else if (is_color_stream && + (format == OB_FORMAT_YUYV || format == OB_FORMAT_UYVY || format == OB_FORMAT_I420 || + format == OB_FORMAT_NV12 || format == OB_FORMAT_NV21)) { + image_format_[stream_index] = CV_8UC3; + encoding_[stream_index] = sensor_msgs::image_encodings::RGB8; + unit_step_size_[stream_index] = 3 * sizeof(uint8_t); } else if (format == OB_FORMAT_MJPG || format == OB_FORMAT_MJPEG) { if (is_ir_stream) { image_format_[stream_index] = CV_8UC1; From ac4346468eb8c51c0b4f9bc732e2ccd5ee621c0f Mon Sep 17 00:00:00 2001 From: slz Date: Tue, 18 Aug 2026 16:34:54 +0800 Subject: [PATCH 2/6] feat: configure image publisher history depth --- .../include/orbbec_camera/ob_camera_node.h | 4 ++ orbbec_camera/launch/astra.launch.py | 6 ++ orbbec_camera/launch/astra2.launch.py | 6 ++ orbbec_camera/launch/dabai_a.launch.py | 8 +++ orbbec_camera/launch/dabai_al.launch.py | 8 +++ orbbec_camera/launch/dabai_dcw2.launch.py | 6 ++ orbbec_camera/launch/dabai_max_pro.launch.py | 6 ++ orbbec_camera/launch/femto.launch.py | 6 ++ orbbec_camera/launch/femto_bolt.launch.py | 6 ++ orbbec_camera/launch/femto_mega.launch.py | 6 ++ orbbec_camera/launch/gemini2.launch.py | 6 ++ orbbec_camera/launch/gemini210.launch.py | 6 ++ orbbec_camera/launch/gemini2L.launch.py | 6 ++ orbbec_camera/launch/gemini345.launch.py | 8 +++ orbbec_camera/launch/gemini345_lg.launch.py | 8 +++ orbbec_camera/launch/gemini435_le.launch.py | 8 +++ .../launch/gemini_301_series.launch.py | 8 +++ .../launch/gemini_330_series.launch.py | 8 +++ .../gemini_330_series_low_cpu.launch.py | 8 +++ .../gemini_330_series_sdk_json.launch.py | 16 ++++++ .../gemini_intra_process_demo_launch.py | 8 +++ orbbec_camera/src/ob_camera_node.cpp | 57 ++++++++++++++----- 22 files changed, 196 insertions(+), 13 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index f21ad31c..ec59c93a 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -324,6 +324,8 @@ class OBCameraNode { void setupImagePublisher(const stream_index_pair& stream_index); + rmw_qos_profile_t getImageQosProfile(const stream_index_pair& stream_index) const; + void setupPipelineConfig(); void setupDiagnosticUpdater(); @@ -692,6 +694,8 @@ class OBCameraNode { std::string camera_link_frame_id_; bool depth_registration_ = false; std::map image_qos_; + std::map image_qos_history_; + std::map image_qos_depth_; std::map camera_info_qos_; std::map format_; std::map format_str_; diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index 0a498db9..3c1e838b 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/orbbec_camera/launch/astra.launch.py @@ -35,6 +35,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_flip', default_value='false'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), DeclareLaunchArgument('color_exposure', default_value='-1'), @@ -48,6 +50,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_flip', default_value='false'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('ir_width', default_value='640'), DeclareLaunchArgument('ir_height', default_value='480'), @@ -56,6 +60,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_ir', default_value='false'), DeclareLaunchArgument('ir_flip', default_value='false'), DeclareLaunchArgument('ir_qos', default_value='default'), + DeclareLaunchArgument('ir_qos_history', default_value='default'), + DeclareLaunchArgument('ir_qos_depth', default_value='-1'), DeclareLaunchArgument('ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index d17f738d..7a97a4e7 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/orbbec_camera/launch/astra2.launch.py @@ -34,6 +34,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_color", default_value="true"), DeclareLaunchArgument("color_flip", default_value="false"), DeclareLaunchArgument("color_qos", default_value="default"), + DeclareLaunchArgument("color_qos_history", default_value="default"), + DeclareLaunchArgument("color_qos_depth", default_value="-1"), DeclareLaunchArgument("color_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"), DeclareLaunchArgument("color_ae_max_exposure", default_value="-1"), @@ -49,6 +51,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_depth", default_value="true"), DeclareLaunchArgument("depth_flip", default_value="false"), DeclareLaunchArgument("depth_qos", default_value="default"), + DeclareLaunchArgument("depth_qos_history", default_value="default"), + DeclareLaunchArgument("depth_qos_depth", default_value="-1"), DeclareLaunchArgument("depth_camera_info_qos", default_value="default"), DeclareLaunchArgument("ir_width", default_value="800"), DeclareLaunchArgument("ir_height", default_value="600"), @@ -57,6 +61,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_ir", default_value="true"), DeclareLaunchArgument("ir_flip", default_value="false"), DeclareLaunchArgument("ir_qos", default_value="default"), + DeclareLaunchArgument("ir_qos_history", default_value="default"), + DeclareLaunchArgument("ir_qos_depth", default_value="-1"), DeclareLaunchArgument("ir_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"), DeclareLaunchArgument("ir_ae_max_exposure", default_value="-1"), diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index 33f2a11c..4d7b904e 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/orbbec_camera/launch/dabai_a.launch.py @@ -94,6 +94,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='ANY'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), @@ -116,6 +118,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='ANY'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('depth_precision', default_value=''), @@ -126,6 +130,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_width', default_value='0'), DeclareLaunchArgument('right_ir_height', default_value='0'), @@ -133,6 +139,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index 1773024c..5640afc6 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/orbbec_camera/launch/dabai_al.launch.py @@ -94,6 +94,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='ANY'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), @@ -116,6 +118,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='ANY'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('depth_precision', default_value=''), @@ -126,6 +130,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_width', default_value='0'), DeclareLaunchArgument('right_ir_height', default_value='0'), @@ -133,6 +139,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py index 95a1eec0..ddd0c11f 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/orbbec_camera/launch/dabai_dcw2.launch.py @@ -35,6 +35,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('flip_color', default_value='false'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), @@ -56,6 +58,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('flip_depth', default_value='false'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), # /config/depthfilter/Openni_device.json，need config path. DeclareLaunchArgument('depth_filter_config', default_value=''), @@ -66,6 +70,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_ir', default_value='true'), DeclareLaunchArgument('flip_ir', default_value='false'), DeclareLaunchArgument('ir_qos', default_value='default'), + DeclareLaunchArgument('ir_qos_history', default_value='default'), + DeclareLaunchArgument('ir_qos_depth', default_value='-1'), DeclareLaunchArgument('ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_max_pro.launch.py b/orbbec_camera/launch/dabai_max_pro.launch.py index 02757996..1e239305 100644 --- a/orbbec_camera/launch/dabai_max_pro.launch.py +++ b/orbbec_camera/launch/dabai_max_pro.launch.py @@ -38,6 +38,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_mirror', default_value='false'), DeclareLaunchArgument('color_rotation', default_value='-1'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), @@ -63,6 +65,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_exposure', default_value='-1'), DeclareLaunchArgument('depth_gain', default_value='-1'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), # /config/depthfilter/Openni_device.json，need config path. DeclareLaunchArgument('depth_filter_config', default_value=''), @@ -73,6 +77,8 @@ def generate_launch_description(): DeclareLaunchArgument('enable_ir', default_value='true'), DeclareLaunchArgument('ir_flip', default_value='false'), DeclareLaunchArgument('ir_qos', default_value='default'), + DeclareLaunchArgument('ir_qos_history', default_value='default'), + DeclareLaunchArgument('ir_qos_depth', default_value='-1'), DeclareLaunchArgument('ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 6ea3f7aa..2da1beac 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/orbbec_camera/launch/femto.launch.py @@ -33,6 +33,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_color", default_value="true"), DeclareLaunchArgument("color_flip", default_value="false"), DeclareLaunchArgument("color_qos", default_value="default"), + DeclareLaunchArgument("color_qos_history", default_value="default"), + DeclareLaunchArgument("color_qos_depth", default_value="-1"), DeclareLaunchArgument("color_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"), DeclareLaunchArgument("color_exposure", default_value="-1"), @@ -48,6 +50,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_depth", default_value="true"), DeclareLaunchArgument("depth_flip", default_value="false"), DeclareLaunchArgument("depth_qos", default_value="default"), + DeclareLaunchArgument("depth_qos_history", default_value="default"), + DeclareLaunchArgument("depth_qos_depth", default_value="-1"), DeclareLaunchArgument("depth_camera_info_qos", default_value="default"), DeclareLaunchArgument("ir_width", default_value="640"), DeclareLaunchArgument("ir_height", default_value="480"), @@ -56,6 +60,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_ir", default_value="true"), DeclareLaunchArgument("ir_flip", default_value="false"), DeclareLaunchArgument("ir_qos", default_value="default"), + DeclareLaunchArgument("ir_qos_history", default_value="default"), + DeclareLaunchArgument("ir_qos_depth", default_value="-1"), DeclareLaunchArgument("ir_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"), DeclareLaunchArgument("ir_exposure", default_value="-1"), diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index 7283f469..cd3cb0b9 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/orbbec_camera/launch/femto_bolt.launch.py @@ -34,6 +34,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_color", default_value="true"), DeclareLaunchArgument("color_flip", default_value="false"), DeclareLaunchArgument("color_qos", default_value="default"), + DeclareLaunchArgument("color_qos_history", default_value="default"), + DeclareLaunchArgument("color_qos_depth", default_value="-1"), DeclareLaunchArgument("color_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"), DeclareLaunchArgument("color_ae_max_exposure", default_value="-1"), @@ -49,6 +51,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_depth", default_value="true"), DeclareLaunchArgument("depth_flip", default_value="false"), DeclareLaunchArgument("depth_qos", default_value="default"), + DeclareLaunchArgument("depth_qos_history", default_value="default"), + DeclareLaunchArgument("depth_qos_depth", default_value="-1"), DeclareLaunchArgument("depth_camera_info_qos", default_value="default"), DeclareLaunchArgument("ir_width", default_value="640"), DeclareLaunchArgument("ir_height", default_value="576"), @@ -57,6 +61,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_ir", default_value="true"), DeclareLaunchArgument("ir_flip", default_value="false"), DeclareLaunchArgument("ir_qos", default_value="default"), + DeclareLaunchArgument("ir_qos_history", default_value="default"), + DeclareLaunchArgument("ir_qos_depth", default_value="-1"), DeclareLaunchArgument("ir_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"), DeclareLaunchArgument("ir_ae_max_exposure", default_value="-1"), diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 040e0696..7a643aa3 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/orbbec_camera/launch/femto_mega.launch.py @@ -34,6 +34,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_color", default_value="true"), DeclareLaunchArgument("color_flip", default_value="false"), DeclareLaunchArgument("color_qos", default_value="default"), + DeclareLaunchArgument("color_qos_history", default_value="default"), + DeclareLaunchArgument("color_qos_depth", default_value="-1"), DeclareLaunchArgument("color_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"), DeclareLaunchArgument("color_ae_max_exposure", default_value="-1"), @@ -49,6 +51,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_depth", default_value="true"), DeclareLaunchArgument("depth_flip", default_value="false"), DeclareLaunchArgument("depth_qos", default_value="default"), + DeclareLaunchArgument("depth_qos_history", default_value="default"), + DeclareLaunchArgument("depth_qos_depth", default_value="-1"), DeclareLaunchArgument("depth_camera_info_qos", default_value="default"), DeclareLaunchArgument("ir_width", default_value="640"), DeclareLaunchArgument("ir_height", default_value="576"), @@ -57,6 +61,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_ir", default_value="true"), DeclareLaunchArgument("ir_flip", default_value="false"), DeclareLaunchArgument("ir_qos", default_value="default"), + DeclareLaunchArgument("ir_qos_history", default_value="default"), + DeclareLaunchArgument("ir_qos_depth", default_value="-1"), DeclareLaunchArgument("ir_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"), DeclareLaunchArgument("ir_ae_max_exposure", default_value="-1"), diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index d0beb879..a64dd3f8 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -34,6 +34,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_color", default_value="true"), DeclareLaunchArgument("color_flip", default_value="false"), DeclareLaunchArgument("color_qos", default_value="default"), + DeclareLaunchArgument("color_qos_history", default_value="default"), + DeclareLaunchArgument("color_qos_depth", default_value="-1"), DeclareLaunchArgument("color_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"), DeclareLaunchArgument("color_ae_max_exposure", default_value="-1"), @@ -57,6 +59,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_depth", default_value="true"), DeclareLaunchArgument("depth_flip", default_value="false"), DeclareLaunchArgument("depth_qos", default_value="default"), + DeclareLaunchArgument("depth_qos_history", default_value="default"), + DeclareLaunchArgument("depth_qos_depth", default_value="-1"), DeclareLaunchArgument("depth_camera_info_qos", default_value="default"), DeclareLaunchArgument("depth_ae_roi_left", default_value="-1"), DeclareLaunchArgument("depth_ae_roi_right", default_value="-1"), @@ -69,6 +73,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_ir", default_value="true"), DeclareLaunchArgument("ir_flip", default_value="false"), DeclareLaunchArgument("ir_qos", default_value="default"), + DeclareLaunchArgument("ir_qos_history", default_value="default"), + DeclareLaunchArgument("ir_qos_depth", default_value="-1"), DeclareLaunchArgument("ir_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"), DeclareLaunchArgument("ir_ae_max_exposure", default_value="-1"), diff --git a/orbbec_camera/launch/gemini210.launch.py b/orbbec_camera/launch/gemini210.launch.py index e5da7d5f..a48c9773 100644 --- a/orbbec_camera/launch/gemini210.launch.py +++ b/orbbec_camera/launch/gemini210.launch.py @@ -34,6 +34,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_color", default_value="true"), DeclareLaunchArgument("color_flip", default_value="false"), DeclareLaunchArgument("color_qos", default_value="default"), + DeclareLaunchArgument("color_qos_history", default_value="default"), + DeclareLaunchArgument("color_qos_depth", default_value="-1"), DeclareLaunchArgument("color_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"), DeclareLaunchArgument("color_ae_max_exposure", default_value="-1"), @@ -55,6 +57,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_depth", default_value="true"), DeclareLaunchArgument("depth_flip", default_value="false"), DeclareLaunchArgument("depth_qos", default_value="default"), + DeclareLaunchArgument("depth_qos_history", default_value="default"), + DeclareLaunchArgument("depth_qos_depth", default_value="-1"), DeclareLaunchArgument("depth_camera_info_qos", default_value="default"), DeclareLaunchArgument("depth_ae_roi_left", default_value="-1"), DeclareLaunchArgument("depth_ae_roi_right", default_value="-1"), @@ -67,6 +71,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_ir", default_value="true"), DeclareLaunchArgument("ir_flip", default_value="false"), DeclareLaunchArgument("ir_qos", default_value="default"), + DeclareLaunchArgument("ir_qos_history", default_value="default"), + DeclareLaunchArgument("ir_qos_depth", default_value="-1"), DeclareLaunchArgument("ir_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"), DeclareLaunchArgument("ir_ae_max_exposure", default_value="-1"), diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index 81405065..bfec3b99 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -96,6 +96,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_color", default_value="true"), DeclareLaunchArgument("color_flip", default_value="false"), DeclareLaunchArgument("color_qos", default_value="default"), + DeclareLaunchArgument("color_qos_history", default_value="default"), + DeclareLaunchArgument("color_qos_depth", default_value="-1"), DeclareLaunchArgument("color_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_color_auto_exposure", default_value="true"), DeclareLaunchArgument("color_ae_max_exposure", default_value="-1"), @@ -115,6 +117,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_depth", default_value="true"), DeclareLaunchArgument("depth_flip", default_value="false"), DeclareLaunchArgument("depth_qos", default_value="default"), + DeclareLaunchArgument("depth_qos_history", default_value="default"), + DeclareLaunchArgument("depth_qos_depth", default_value="-1"), DeclareLaunchArgument("depth_camera_info_qos", default_value="default"), DeclareLaunchArgument("ir_width", default_value="640"), DeclareLaunchArgument("ir_height", default_value="400"), @@ -123,6 +127,8 @@ def generate_launch_description(): DeclareLaunchArgument("enable_ir", default_value="true"), DeclareLaunchArgument("ir_flip", default_value="false"), DeclareLaunchArgument("ir_qos", default_value="default"), + DeclareLaunchArgument("ir_qos_history", default_value="default"), + DeclareLaunchArgument("ir_qos_depth", default_value="-1"), DeclareLaunchArgument("ir_camera_info_qos", default_value="default"), DeclareLaunchArgument("enable_ir_auto_exposure", default_value="true"), DeclareLaunchArgument("ir_ae_max_exposure", default_value="-1"), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index fd1e3167..29efb1ad 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/orbbec_camera/launch/gemini345.launch.py @@ -93,6 +93,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='ANY'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), @@ -115,6 +117,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='ANY'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('depth_precision', default_value=''), @@ -125,6 +129,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_width', default_value='0'), DeclareLaunchArgument('right_ir_height', default_value='0'), @@ -132,6 +138,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index 87e46fdd..a5ec7a15 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/orbbec_camera/launch/gemini345_lg.launch.py @@ -94,6 +94,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='ANY'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), @@ -116,6 +118,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='ANY'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('depth_precision', default_value=''), @@ -126,6 +130,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_width', default_value='0'), DeclareLaunchArgument('right_ir_height', default_value='0'), @@ -133,6 +139,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py index 726472be..2eca0b27 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/orbbec_camera/launch/gemini435_le.launch.py @@ -94,6 +94,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='ANY'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('color_rotation', default_value='-1'),#color rotation degree : 0, 90, 180, 270 @@ -126,6 +128,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='ANY'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('depth_precision', default_value=''), @@ -143,6 +147,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('left_ir_rotation', default_value='-1'),#left_ir rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('left_ir_flip', default_value='false'), @@ -153,6 +159,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_rotation', default_value='-1'),#right_ir rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('right_ir_flip', default_value='false'), diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 4339900b..23e57da5 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -119,6 +119,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='YUYV'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('color_rotation', default_value='-1'),#color rotation degree : 0, 90, 180, 270 @@ -155,6 +157,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='Y16'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('depth_precision', default_value=''), @@ -173,6 +177,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='Y8'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('left_ir_rotation', default_value='-1'),#left_ir rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('left_ir_flip', default_value='false'), @@ -186,6 +192,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='Y8'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_rotation', default_value='-1'),#right_ir rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('right_ir_flip', default_value='false'), diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index a8860183..bcce78e2 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -108,6 +108,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='ANY'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('color_rotation', default_value='-1'),#color rotation degree : 0, 90, 180, 270 @@ -144,6 +146,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='ANY'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('depth_precision', default_value=''), @@ -161,6 +165,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('left_ir_rotation', default_value='-1'),#left_ir rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('left_ir_flip', default_value='false'), @@ -173,6 +179,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_rotation', default_value='-1'),#right_ir rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('right_ir_flip', default_value='false'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index 36be361e..7089840e 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py @@ -96,6 +96,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='ANY'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('color_rotation', default_value='-1'),#color rotation degree : 0, 90, 180, 270 @@ -132,6 +134,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='ANY'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('depth_precision', default_value=''), @@ -149,6 +153,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('left_ir_rotation', default_value='-1'),#left_ir rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('left_ir_flip', default_value='false'), @@ -161,6 +167,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_rotation', default_value='-1'),#right_ir rotation degree : 0, 90, 180, 270 DeclareLaunchArgument('right_ir_flip', default_value='false'), diff --git a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py index a41be6b1..7a8e862e 100644 --- a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py +++ b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py @@ -83,15 +83,31 @@ def generate_launch_description(): DeclareLaunchArgument('export_config_json_file_path', default_value=''), DeclareLaunchArgument('color_qos', default_value='default'), + + DeclareLaunchArgument('color_qos_history', default_value='default'), + + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('depth_qos', default_value='default'), + + DeclareLaunchArgument('depth_qos_history', default_value='default'), + + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'), diff --git a/orbbec_camera/launch/gemini_intra_process_demo_launch.py b/orbbec_camera/launch/gemini_intra_process_demo_launch.py index 21cc4ae2..43ec8282 100644 --- a/orbbec_camera/launch/gemini_intra_process_demo_launch.py +++ b/orbbec_camera/launch/gemini_intra_process_demo_launch.py @@ -70,6 +70,8 @@ def generate_launch_description(): DeclareLaunchArgument('color_format', default_value='ANY'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_qos_history', default_value='default'), + DeclareLaunchArgument('color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), DeclareLaunchArgument('color_exposure', default_value='-1'), @@ -82,6 +84,8 @@ def generate_launch_description(): DeclareLaunchArgument('depth_format', default_value='ANY'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_qos_history', default_value='default'), + DeclareLaunchArgument('depth_qos_depth', default_value='-1'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), DeclareLaunchArgument('left_ir_width', default_value='0'), DeclareLaunchArgument('left_ir_height', default_value='0'), @@ -89,6 +93,8 @@ def generate_launch_description(): DeclareLaunchArgument('left_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_left_ir', default_value='false'), DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_qos_history', default_value='default'), + DeclareLaunchArgument('left_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('right_ir_width', default_value='0'), DeclareLaunchArgument('right_ir_height', default_value='0'), @@ -96,6 +102,8 @@ def generate_launch_description(): DeclareLaunchArgument('right_ir_format', default_value='ANY'), DeclareLaunchArgument('enable_right_ir', default_value='false'), DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_qos_history', default_value='default'), + DeclareLaunchArgument('right_ir_qos_depth', default_value='-1'), DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), DeclareLaunchArgument('ir_exposure', default_value='-1'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 1aa56019..925726f5 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -16,6 +16,7 @@ #include "orbbec_camera/ob_camera_node.h" #include +#include #include #include #include @@ -4415,6 +4416,20 @@ void OBCameraNode::getParameters() { updateImageConfig(stream_index); param_name = stream_name_[stream_index] + "_qos"; setAndGetNodeParameter(image_qos_[stream_index], param_name, "default"); + param_name = stream_name_[stream_index] + "_qos_history"; + setAndGetNodeParameter(image_qos_history_[stream_index], param_name, "default"); + std::transform(image_qos_history_[stream_index].begin(), image_qos_history_[stream_index].end(), + image_qos_history_[stream_index].begin(), ::toupper); + if (image_qos_history_[stream_index] != "DEFAULT" && + image_qos_history_[stream_index] != "KEEP_LAST" && + image_qos_history_[stream_index] != "KEEP_ALL") { + throw std::invalid_argument(param_name + " must be DEFAULT, KEEP_LAST, or KEEP_ALL"); + } + param_name = stream_name_[stream_index] + "_qos_depth"; + setAndGetNodeParameter(image_qos_depth_[stream_index], param_name, -1); + if (image_qos_depth_[stream_index] == 0 || image_qos_depth_[stream_index] < -1) { + throw std::invalid_argument(param_name + " must be -1 or greater than zero"); + } param_name = stream_name_[stream_index] + "_camera_info_qos"; setAndGetNodeParameter(camera_info_qos_[stream_index], param_name, "default"); param_name = "enable_" + stream_name_[stream_index] + "_undistortion"; @@ -5267,10 +5282,7 @@ void OBCameraNode::setupConfidencePublishers() { if (confidence_image_publisher_) { return; } - auto image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]); - if (use_intra_process_) { - image_qos_profile = rmw_qos_profile_default; - } + const auto image_qos_profile = getImageQosProfile(DEPTH); confidence_image_publisher_ = node_->create_publisher( "confidence/image_raw", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile)); @@ -5359,11 +5371,7 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { } const std::string topic = stream_name_[stream_index] + "/image_raw"; - auto image_qos_profile = getRMWQosProfileFromString(image_qos_[stream_index]); - if (use_intra_process_) { - image_qos_profile = rmw_qos_profile_default; - } - + const auto image_qos_profile = getImageQosProfile(stream_index); const bool is_mjpg_color_stream = (stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) && format_[stream_index] == OB_FORMAT_MJPG; @@ -5374,6 +5382,14 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { image_publishers_[stream_index] = std::make_shared(*node_, topic, image_qos_profile); } + std::string history = rmw_qos_history_policy_to_str(image_qos_profile.history); + if (image_qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { + history += "(" + std::to_string(image_qos_profile.depth) + ")"; + } + RCLCPP_INFO_STREAM( + logger_, topic << " QoS: " << rmw_qos_reliability_policy_to_str(image_qos_profile.reliability) + << ", " << rmw_qos_durability_policy_to_str(image_qos_profile.durability) + << ", " << history); if (is_mjpg_color_stream) { compressed_image_publishers_[stream_index] = @@ -5385,6 +5401,24 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { } } +rmw_qos_profile_t OBCameraNode::getImageQosProfile(const stream_index_pair &stream_index) const { + auto image_qos_profile = getRMWQosProfileFromString(image_qos_.at(stream_index)); + if (use_intra_process_) { + image_qos_profile = rmw_qos_profile_default; + } + const auto &history = image_qos_history_.at(stream_index); + if (history == "KEEP_LAST") { + image_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + } else if (history == "KEEP_ALL") { + image_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + } + const auto depth = image_qos_depth_.at(stream_index); + if (depth > 0) { + image_qos_profile.depth = static_cast(depth); + } + return image_qos_profile; +} + void OBCameraNode::setupPublishers() { using PointCloud2 = sensor_msgs::msg::PointCloud2; using CameraInfo = sensor_msgs::msg::CameraInfo; @@ -5529,10 +5563,7 @@ void OBCameraNode::syncSoftwareAlignment() { RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_); } if (!depth_unaligned_publisher_) { - auto depth_image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]); - if (use_intra_process_) { - depth_image_qos_profile = rmw_qos_profile_default; - } + const auto depth_image_qos_profile = getImageQosProfile(DEPTH); if (use_intra_process_) { depth_unaligned_publisher_ = std::make_shared( *node_, "depth/image_unaligned", depth_image_qos_profile); From c141430c62e22d2b28cf564d211a56b3c2837c21 Mon Sep 17 00:00:00 2001 From: slz Date: Wed, 19 Aug 2026 10:27:58 +0800 Subject: [PATCH 3/6] feat: bound and diagnose color frame queues --- .../include/orbbec_camera/ob_camera_node.h | 44 ++++- orbbec_camera/launch/astra.launch.py | 1 + orbbec_camera/launch/astra2.launch.py | 1 + orbbec_camera/launch/dabai_a.launch.py | 1 + orbbec_camera/launch/dabai_al.launch.py | 1 + orbbec_camera/launch/dabai_dcw2.launch.py | 1 + orbbec_camera/launch/dabai_max_pro.launch.py | 1 + orbbec_camera/launch/femto.launch.py | 1 + orbbec_camera/launch/femto_bolt.launch.py | 1 + orbbec_camera/launch/femto_mega.launch.py | 1 + orbbec_camera/launch/gemini2.launch.py | 1 + orbbec_camera/launch/gemini210.launch.py | 1 + orbbec_camera/launch/gemini2L.launch.py | 1 + orbbec_camera/launch/gemini345.launch.py | 1 + orbbec_camera/launch/gemini345_lg.launch.py | 1 + orbbec_camera/launch/gemini435_le.launch.py | 1 + .../launch/gemini_301_series.launch.py | 3 + .../launch/gemini_330_series.launch.py | 1 + .../gemini_330_series_low_cpu.launch.py | 1 + .../gemini_330_series_sdk_json.launch.py | 2 + .../gemini_intra_process_demo_launch.py | 1 + orbbec_camera/src/ob_camera_node.cpp | 172 ++++++++++++++---- orbbec_camera/src/ros_service.cpp | 47 +++++ 23 files changed, 244 insertions(+), 42 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index ec59c93a..d80a6f18 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -18,9 +18,12 @@ #include +#include #include +#include #include #include +#include #include #include #include @@ -334,6 +337,9 @@ class OBCameraNode { void setupCameraCtrlServices(); + void getColorQueueStatsCallback(const std::shared_ptr& request, + std::shared_ptr& response); + void stopStreams(); void stopIMU(); @@ -772,6 +778,7 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_sync_io_voltage_level_srv_; rclcpp::Service::SharedPtr get_streams_enable_srv_; rclcpp::Service::SharedPtr set_streams_enable_srv_; + rclcpp::Service::SharedPtr get_color_queue_stats_srv_; rclcpp::Service::SharedPtr set_image_registration_mode_srv_; rclcpp::Service::SharedPtr set_stream_profile_srv_; rclcpp::Service::SharedPtr get_user_calib_params_srv_; @@ -915,21 +922,52 @@ class OBCameraNode { bool is_right_color_frame_decoded_ = false; bool is_color_frame_decoded_ = false; std::recursive_mutex device_lock_; + struct QueuedColorFrame { + std::shared_ptr frame_set; + std::chrono::steady_clock::time_point enqueue_time; + }; + struct ColorQueueStats { + size_t max_queue_size = 0; + uint64_t overflow_count = 0; + double max_queue_wait_ms = 0.0; + }; + struct ColorQueueStatsSnapshot { + int capacity_frames = 0; + size_t queue_size = 0; + size_t max_queue_size = 0; + uint64_t overflow_count = 0; + double oldest_queue_wait_ms = 0.0; + double max_queue_wait_ms = 0.0; + }; + using ColorFrameQueue = std::queue; + void enqueueColorFrame(ColorFrameQueue& queue, std::mutex& mutex, + std::condition_variable& condition_variable, ColorQueueStats& stats, + int capacity_frames, const std::shared_ptr& frame_set, + const char* queue_name); + ColorQueueStatsSnapshot getColorQueueStats(ColorFrameQueue& queue, std::mutex& mutex, + ColorQueueStats& stats, int capacity_frames, + bool reset); // For color - std::queue> color_frame_queue_; + ColorFrameQueue color_frame_queue_; + ColorQueueStats color_frame_queue_stats_; + int color_frame_queue_max_frames_ = 10; std::shared_ptr colorFrameThread_ = nullptr; std::atomic_bool stop_color_frame_threads_{false}; std::mutex color_frame_queue_lock_; std::condition_variable color_frame_queue_cv_; // For left color - std::queue> left_color_frame_queue_; + ColorFrameQueue left_color_frame_queue_; + ColorQueueStats left_color_frame_queue_stats_; + int left_color_frame_queue_max_frames_ = 10; std::shared_ptr leftColorFrameThread_ = nullptr; std::mutex left_color_frame_queue_lock_; std::condition_variable left_color_frame_queue_cv_; // For right color - std::queue> right_color_frame_queue_; + ColorFrameQueue right_color_frame_queue_; + ColorQueueStats right_color_frame_queue_stats_; + int right_color_frame_queue_max_frames_ = 10; std::shared_ptr rightColorFrameThread_ = nullptr; std::mutex right_color_frame_queue_lock_; std::condition_variable right_color_frame_queue_cv_; diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index 3c1e838b..8921c357 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/orbbec_camera/launch/astra.launch.py @@ -28,6 +28,7 @@ def generate_launch_description(): DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('point_cloud_qos', default_value='default'), DeclareLaunchArgument('connection_delay', default_value='100'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='640'), DeclareLaunchArgument('color_height', default_value='480'), DeclareLaunchArgument('color_fps', default_value='10'), diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index 7a97a4e7..8dc5e316 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/orbbec_camera/launch/astra2.launch.py @@ -27,6 +27,7 @@ def generate_launch_description(): DeclareLaunchArgument("cloud_frame_id", default_value=""), DeclareLaunchArgument("point_cloud_qos", default_value="default"), DeclareLaunchArgument("connection_delay", default_value="100"), + DeclareLaunchArgument("color_frame_queue_max_frames", default_value="10"), DeclareLaunchArgument("color_width", default_value="1280"), DeclareLaunchArgument("color_height", default_value="720"), DeclareLaunchArgument("color_fps", default_value="30"), diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index 4d7b904e..2151043d 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/orbbec_camera/launch/dabai_a.launch.py @@ -88,6 +88,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='0'), DeclareLaunchArgument('color_height', default_value='0'), DeclareLaunchArgument('color_fps', default_value='0'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index 5640afc6..42f708b1 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/orbbec_camera/launch/dabai_al.launch.py @@ -88,6 +88,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='0'), DeclareLaunchArgument('color_height', default_value='0'), DeclareLaunchArgument('color_fps', default_value='0'), diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py index ddd0c11f..2bdf6716 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/orbbec_camera/launch/dabai_dcw2.launch.py @@ -28,6 +28,7 @@ def generate_launch_description(): DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('point_cloud_qos', default_value='default'), DeclareLaunchArgument('connection_delay', default_value='100'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='640'), DeclareLaunchArgument('color_height', default_value='480'), DeclareLaunchArgument('color_fps', default_value='15'), diff --git a/orbbec_camera/launch/dabai_max_pro.launch.py b/orbbec_camera/launch/dabai_max_pro.launch.py index 1e239305..7abbbe84 100644 --- a/orbbec_camera/launch/dabai_max_pro.launch.py +++ b/orbbec_camera/launch/dabai_max_pro.launch.py @@ -29,6 +29,7 @@ def generate_launch_description(): DeclareLaunchArgument('point_cloud_qos', default_value='default'), DeclareLaunchArgument('connection_delay', default_value='100'), DeclareLaunchArgument('diagnostic_period', default_value='0.0'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='640'), DeclareLaunchArgument('color_height', default_value='480'), DeclareLaunchArgument('color_fps', default_value='25'), diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 2da1beac..096de049 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/orbbec_camera/launch/femto.launch.py @@ -26,6 +26,7 @@ def generate_launch_description(): DeclareLaunchArgument("enable_colored_point_cloud", default_value="false"), DeclareLaunchArgument("point_cloud_qos", default_value="default"), DeclareLaunchArgument("connection_delay", default_value="100"), + DeclareLaunchArgument("color_frame_queue_max_frames", default_value="10"), DeclareLaunchArgument("color_width", default_value="640"), DeclareLaunchArgument("color_height", default_value="480"), DeclareLaunchArgument("color_fps", default_value="30"), diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index cd3cb0b9..ad857ee6 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/orbbec_camera/launch/femto_bolt.launch.py @@ -27,6 +27,7 @@ def generate_launch_description(): DeclareLaunchArgument("enable_colored_point_cloud", default_value="false"), DeclareLaunchArgument("point_cloud_qos", default_value="default"), DeclareLaunchArgument("connection_delay", default_value="100"), + DeclareLaunchArgument("color_frame_queue_max_frames", default_value="10"), DeclareLaunchArgument("color_width", default_value="1280"), DeclareLaunchArgument("color_height", default_value="720"), DeclareLaunchArgument("color_fps", default_value="30"), diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 7a643aa3..b882d16b 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/orbbec_camera/launch/femto_mega.launch.py @@ -27,6 +27,7 @@ def generate_launch_description(): DeclareLaunchArgument("enable_colored_point_cloud", default_value="false"), DeclareLaunchArgument("point_cloud_qos", default_value="default"), DeclareLaunchArgument("connection_delay", default_value="100"), + DeclareLaunchArgument("color_frame_queue_max_frames", default_value="10"), DeclareLaunchArgument("color_width", default_value="1280"), DeclareLaunchArgument("color_height", default_value="720"), DeclareLaunchArgument("color_fps", default_value="30"), diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index a64dd3f8..21c5f8e3 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -27,6 +27,7 @@ def generate_launch_description(): DeclareLaunchArgument("enable_colored_point_cloud", default_value="false"), DeclareLaunchArgument("point_cloud_qos", default_value="default"), DeclareLaunchArgument("connection_delay", default_value="100"), + DeclareLaunchArgument("color_frame_queue_max_frames", default_value="10"), DeclareLaunchArgument("color_width", default_value="0"), DeclareLaunchArgument("color_height", default_value="0"), DeclareLaunchArgument("color_fps", default_value="0"), diff --git a/orbbec_camera/launch/gemini210.launch.py b/orbbec_camera/launch/gemini210.launch.py index a48c9773..23bda161 100644 --- a/orbbec_camera/launch/gemini210.launch.py +++ b/orbbec_camera/launch/gemini210.launch.py @@ -27,6 +27,7 @@ def generate_launch_description(): DeclareLaunchArgument("enable_colored_point_cloud", default_value="false"), DeclareLaunchArgument("point_cloud_qos", default_value="default"), DeclareLaunchArgument("connection_delay", default_value="100"), + DeclareLaunchArgument("color_frame_queue_max_frames", default_value="10"), DeclareLaunchArgument("color_width", default_value="0"), DeclareLaunchArgument("color_height", default_value="0"), DeclareLaunchArgument("color_fps", default_value="0"), diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index bfec3b99..8dec1b3b 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -89,6 +89,7 @@ def generate_launch_description(): DeclareLaunchArgument("enable_colored_point_cloud", default_value="false"), DeclareLaunchArgument("point_cloud_qos", default_value="default"), DeclareLaunchArgument("connection_delay", default_value="100"), + DeclareLaunchArgument("color_frame_queue_max_frames", default_value="10"), DeclareLaunchArgument("color_width", default_value="1280"), DeclareLaunchArgument("color_height", default_value="800"), DeclareLaunchArgument("color_fps", default_value="30"), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index 29efb1ad..d59befc0 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/orbbec_camera/launch/gemini345.launch.py @@ -87,6 +87,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='0'), DeclareLaunchArgument('color_height', default_value='0'), DeclareLaunchArgument('color_fps', default_value='0'), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index a5ec7a15..947c416f 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/orbbec_camera/launch/gemini345_lg.launch.py @@ -88,6 +88,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='0'), DeclareLaunchArgument('color_height', default_value='0'), DeclareLaunchArgument('color_fps', default_value='0'), diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py index 2eca0b27..d2ad1764 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/orbbec_camera/launch/gemini435_le.launch.py @@ -88,6 +88,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='0'), DeclareLaunchArgument('color_height', default_value='0'), DeclareLaunchArgument('color_fps', default_value='0'), diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 23e57da5..751f87cb 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -111,6 +111,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), + DeclareLaunchArgument('left_color_frame_queue_max_frames', default_value='10'), + DeclareLaunchArgument('right_color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('ae_reference_stream', default_value='depth'), # depth or color DeclareLaunchArgument('ae_strategy', default_value='motion'), # default or motion DeclareLaunchArgument('color_width', default_value='848'), diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index bcce78e2..edbdfb75 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -102,6 +102,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='0'), DeclareLaunchArgument('color_height', default_value='0'), DeclareLaunchArgument('color_fps', default_value='0'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index 7089840e..922d9974 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py @@ -90,6 +90,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='0'), DeclareLaunchArgument('color_height', default_value='0'), DeclareLaunchArgument('color_fps', default_value='0'), diff --git a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py index 7a8e862e..b3e65f3d 100644 --- a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py +++ b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py @@ -82,6 +82,8 @@ def generate_launch_description(): DeclareLaunchArgument('load_config_json_file_path', default_value=''), DeclareLaunchArgument('export_config_json_file_path', default_value=''), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), + DeclareLaunchArgument('color_qos', default_value='default'), DeclareLaunchArgument('color_qos_history', default_value='default'), diff --git a/orbbec_camera/launch/gemini_intra_process_demo_launch.py b/orbbec_camera/launch/gemini_intra_process_demo_launch.py index 43ec8282..1ecf1b7f 100644 --- a/orbbec_camera/launch/gemini_intra_process_demo_launch.py +++ b/orbbec_camera/launch/gemini_intra_process_demo_launch.py @@ -64,6 +64,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'), DeclareLaunchArgument('cloud_frame_id', default_value=''), DeclareLaunchArgument('connection_delay', default_value='10'), + DeclareLaunchArgument('color_frame_queue_max_frames', default_value='10'), DeclareLaunchArgument('color_width', default_value='0'), DeclareLaunchArgument('color_height', default_value='0'), DeclareLaunchArgument('color_fps', default_value='0'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 925726f5..f0c67931 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -3811,17 +3811,17 @@ bool OBCameraNode::applyStreamProfiles(const std::vector & void OBCameraNode::clearColorFrameQueues() { { std::lock_guard lock(color_frame_queue_lock_); - std::queue> empty; + ColorFrameQueue empty; std::swap(color_frame_queue_, empty); } { std::lock_guard lock(left_color_frame_queue_lock_); - std::queue> empty; + ColorFrameQueue empty; std::swap(left_color_frame_queue_, empty); } { std::lock_guard lock(right_color_frame_queue_lock_); - std::queue> empty; + ColorFrameQueue empty; std::swap(right_color_frame_queue_, empty); } is_color_frame_decoded_ = false; @@ -3829,6 +3829,56 @@ void OBCameraNode::clearColorFrameQueues() { is_right_color_frame_decoded_ = false; } +void OBCameraNode::enqueueColorFrame(ColorFrameQueue &queue, std::mutex &mutex, + std::condition_variable &condition_variable, + ColorQueueStats &stats, int capacity_frames, + const std::shared_ptr &frame_set, + const char *queue_name) { + uint64_t overflow_count = 0; + { + std::lock_guard lock(mutex); + const auto now = std::chrono::steady_clock::now(); + if (queue.size() >= static_cast(capacity_frames)) { + const auto oldest_age = + std::chrono::duration(now - queue.front().enqueue_time).count(); + stats.max_queue_wait_ms = std::max(stats.max_queue_wait_ms, oldest_age); + queue.pop(); + overflow_count = ++stats.overflow_count; + } + queue.push(QueuedColorFrame{frame_set, now}); + stats.max_queue_size = std::max(stats.max_queue_size, queue.size()); + } + condition_variable.notify_one(); + if (overflow_count == 1 || (overflow_count > 0 && overflow_count % 100 == 0)) { + RCLCPP_WARN_STREAM( + logger_, "Color frame queue overflow: queue=" << queue_name << " count=" << overflow_count + << " capacity_frames=" << capacity_frames); + } +} + +OBCameraNode::ColorQueueStatsSnapshot OBCameraNode::getColorQueueStats(ColorFrameQueue &queue, + std::mutex &mutex, + ColorQueueStats &stats, + int capacity_frames, + bool reset) { + std::lock_guard lock(mutex); + const auto oldest_queue_wait_ms = + queue.empty() ? 0.0 + : std::chrono::duration(std::chrono::steady_clock::now() - + queue.front().enqueue_time) + .count(); + stats.max_queue_wait_ms = std::max(stats.max_queue_wait_ms, oldest_queue_wait_ms); + const ColorQueueStatsSnapshot snapshot{capacity_frames, queue.size(), + stats.max_queue_size, stats.overflow_count, + oldest_queue_wait_ms, stats.max_queue_wait_ms}; + if (reset) { + stats.max_queue_size = queue.size(); + stats.overflow_count = 0; + stats.max_queue_wait_ms = oldest_queue_wait_ms; + } + return snapshot; +} + void OBCameraNode::stopColorFrameThreads() { if (!colorFrameThread_ && !leftColorFrameThread_ && !rightColorFrameThread_) { return; @@ -4383,6 +4433,20 @@ void OBCameraNode::setupDefaultImageFormat() { void OBCameraNode::getParameters() { setAndGetNodeParameter(camera_name_, "camera_name", "camera"); + setAndGetNodeParameter(color_frame_queue_max_frames_, "color_frame_queue_max_frames", 10); + setAndGetNodeParameter(left_color_frame_queue_max_frames_, + "left_color_frame_queue_max_frames", 10); + setAndGetNodeParameter(right_color_frame_queue_max_frames_, + "right_color_frame_queue_max_frames", 10); + const auto validate_queue_capacity = [](const char *name, int capacity) { + if (capacity < 1) { + throw std::invalid_argument(std::string(name) + " must be greater than zero"); + } + }; + validate_queue_capacity("color_frame_queue_max_frames", color_frame_queue_max_frames_); + validate_queue_capacity("left_color_frame_queue_max_frames", left_color_frame_queue_max_frames_); + validate_queue_capacity("right_color_frame_queue_max_frames", + right_color_frame_queue_max_frames_); camera_link_frame_id_ = camera_name_ + "_link"; for (auto stream_index : IMAGE_STREAMS) { std::string param_name = stream_name_[stream_index] + "_width"; @@ -6302,22 +6366,22 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set } if (enable_stream_[COLOR] && color_frame) { - std::unique_lock lock(color_frame_queue_lock_); - color_frame_queue_.push(frame_set); - color_frame_queue_cv_.notify_all(); + enqueueColorFrame(color_frame_queue_, color_frame_queue_lock_, color_frame_queue_cv_, + color_frame_queue_stats_, color_frame_queue_max_frames_, frame_set, + "color"); } else { publishPointCloud(frame_set); } if (enable_stream_[COLOR_LEFT] && left_color_frame) { - std::unique_lock lock(left_color_frame_queue_lock_); - left_color_frame_queue_.push(frame_set); - left_color_frame_queue_cv_.notify_all(); + enqueueColorFrame(left_color_frame_queue_, left_color_frame_queue_lock_, + left_color_frame_queue_cv_, left_color_frame_queue_stats_, + left_color_frame_queue_max_frames_, frame_set, "left_color"); } if (enable_stream_[COLOR_RIGHT] && right_color_frame) { - std::unique_lock lock(right_color_frame_queue_lock_); - right_color_frame_queue_.push(frame_set); - right_color_frame_queue_cv_.notify_all(); + enqueueColorFrame(right_color_frame_queue_, right_color_frame_queue_lock_, + right_color_frame_queue_cv_, right_color_frame_queue_stats_, + right_color_frame_queue_max_frames_, frame_set, "right_color"); } for (const auto &stream_index : IMAGE_STREAMS) { @@ -6369,20 +6433,30 @@ void OBCameraNode::logFrameInfoOnce(const stream_index_pair &stream_index, void OBCameraNode::onNewColorFrameCallback() { while (enable_stream_[COLOR] && rclcpp::ok() && is_running_.load() && !stop_color_frame_threads_.load()) { - std::unique_lock lock(color_frame_queue_lock_); - color_frame_queue_cv_.wait(lock, [this]() { - return !color_frame_queue_.empty() || !(is_running_.load()) || - stop_color_frame_threads_.load(); - }); + std::shared_ptr frameSet; + { + std::unique_lock lock(color_frame_queue_lock_); + color_frame_queue_cv_.wait(lock, [this]() { + return !color_frame_queue_.empty() || !(is_running_.load()) || + stop_color_frame_threads_.load(); + }); - if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) { - break; + if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) { + break; + } + const auto queued = color_frame_queue_.front(); + color_frame_queue_.pop(); + frameSet = queued.frame_set; + color_frame_queue_stats_.max_queue_wait_ms = + std::max(color_frame_queue_stats_.max_queue_wait_ms, + std::chrono::duration(std::chrono::steady_clock::now() - + queued.enqueue_time) + .count()); } - std::shared_ptr frameSet = color_frame_queue_.front(); + is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_); onNewFrameCallback(frameSet->colorFrame(), COLOR); publishPointCloud(frameSet); - color_frame_queue_.pop(); } RCLCPP_DEBUG_STREAM(logger_, "Color frame thread exited"); @@ -6391,20 +6465,30 @@ void OBCameraNode::onNewColorFrameCallback() { void OBCameraNode::onNewLeftColorFrameCallback() { while (enable_stream_[COLOR_LEFT] && rclcpp::ok() && is_running_.load() && !stop_color_frame_threads_.load()) { - std::unique_lock lock(left_color_frame_queue_lock_); - left_color_frame_queue_cv_.wait(lock, [this]() { - return !left_color_frame_queue_.empty() || !(is_running_.load()) || - stop_color_frame_threads_.load(); - }); + std::shared_ptr frameSet; + { + std::unique_lock lock(left_color_frame_queue_lock_); + left_color_frame_queue_cv_.wait(lock, [this]() { + return !left_color_frame_queue_.empty() || !(is_running_.load()) || + stop_color_frame_threads_.load(); + }); - if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) { - break; + if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) { + break; + } + const auto queued = left_color_frame_queue_.front(); + left_color_frame_queue_.pop(); + frameSet = queued.frame_set; + left_color_frame_queue_stats_.max_queue_wait_ms = + std::max(left_color_frame_queue_stats_.max_queue_wait_ms, + std::chrono::duration(std::chrono::steady_clock::now() - + queued.enqueue_time) + .count()); } - std::shared_ptr frameSet = left_color_frame_queue_.front(); + is_left_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->getFrame(OB_FRAME_COLOR_LEFT), rgb_buffer_left_); onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_LEFT), COLOR_LEFT); - left_color_frame_queue_.pop(); } RCLCPP_DEBUG_STREAM(logger_, "Left color frame thread exited"); } @@ -6412,20 +6496,30 @@ void OBCameraNode::onNewLeftColorFrameCallback() { void OBCameraNode::onNewRightColorFrameCallback() { while (enable_stream_[COLOR_RIGHT] && rclcpp::ok() && is_running_.load() && !stop_color_frame_threads_.load()) { - std::unique_lock lock(right_color_frame_queue_lock_); - right_color_frame_queue_cv_.wait(lock, [this]() { - return !right_color_frame_queue_.empty() || !(is_running_.load()) || - stop_color_frame_threads_.load(); - }); + std::shared_ptr frameSet; + { + std::unique_lock lock(right_color_frame_queue_lock_); + right_color_frame_queue_cv_.wait(lock, [this]() { + return !right_color_frame_queue_.empty() || !(is_running_.load()) || + stop_color_frame_threads_.load(); + }); - if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) { - break; + if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) { + break; + } + const auto queued = right_color_frame_queue_.front(); + right_color_frame_queue_.pop(); + frameSet = queued.frame_set; + right_color_frame_queue_stats_.max_queue_wait_ms = + std::max(right_color_frame_queue_stats_.max_queue_wait_ms, + std::chrono::duration(std::chrono::steady_clock::now() - + queued.enqueue_time) + .count()); } - std::shared_ptr frameSet = right_color_frame_queue_.front(); + is_right_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), rgb_buffer_right_); onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), COLOR_RIGHT); - right_color_frame_queue_.pop(); } RCLCPP_DEBUG_STREAM(logger_, "Right color frame thread exited"); } diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 8c4be54b..5a8a3fbf 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -121,6 +121,11 @@ std::string OBSyncModeToString(const OBMultiDeviceSyncMode& mode) { void OBCameraNode::setupCameraCtrlServices() { using std_srvs::srv::SetBool; + get_color_queue_stats_srv_ = node_->create_service( + "get_color_queue_stats", [this](const std::shared_ptr request, + std::shared_ptr response) { + getColorQueueStatsCallback(request, response); + }); for (auto stream_index : IMAGE_STREAMS) { if (!enable_stream_[stream_index]) { continue; @@ -458,6 +463,48 @@ void OBCameraNode::setupCameraCtrlServices() { } } +void OBCameraNode::getColorQueueStatsCallback( + const std::shared_ptr& request, + std::shared_ptr& response) { + try { + const auto to_json = [](const ColorQueueStatsSnapshot& stats) { + return nlohmann::json{ + {"capacity_frames", stats.capacity_frames}, + {"queue_size", stats.queue_size}, + {"max_queue_size", stats.max_queue_size}, + {"overflow_count", stats.overflow_count}, + {"oldest_queue_wait_ms", stats.oldest_queue_wait_ms}, + {"max_queue_wait_ms", stats.max_queue_wait_ms}, + }; + }; + nlohmann::json queues; + queues["color"] = to_json(getColorQueueStats(color_frame_queue_, color_frame_queue_lock_, + color_frame_queue_stats_, + color_frame_queue_max_frames_, request->data)); + queues["left_color"] = to_json(getColorQueueStats( + left_color_frame_queue_, left_color_frame_queue_lock_, left_color_frame_queue_stats_, + left_color_frame_queue_max_frames_, request->data)); + queues["right_color"] = to_json(getColorQueueStats( + right_color_frame_queue_, right_color_frame_queue_lock_, right_color_frame_queue_stats_, + right_color_frame_queue_max_frames_, request->data)); + const uint64_t overflow_count = queues["color"]["overflow_count"].get() + + queues["left_color"]["overflow_count"].get() + + queues["right_color"]["overflow_count"].get(); + response->success = true; + response->message = + nlohmann::json{ + {"namespace", node_->get_namespace()}, + {"overflow_count", overflow_count}, + {"statistics_reset", request->data}, + {"queues", queues}, + } + .dump(); + } catch (const std::exception& error) { + response->success = false; + response->message = error.what(); + } +} + void OBCameraNode::getPointCloudDecimationCallback( const std::shared_ptr& request, std::shared_ptr& response) { From db4df46c0370837951a57766ebbd2005e6ae1179 Mon Sep 17 00:00:00 2001 From: slz Date: Wed, 19 Aug 2026 16:35:13 +0800 Subject: [PATCH 4/6] fix: report only enabled color queue statistics --- orbbec_camera/src/ros_service.cpp | 38 +++++++++++++++++++++---------- 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 5a8a3fbf..01af1b51 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -477,19 +477,30 @@ void OBCameraNode::getColorQueueStatsCallback( {"max_queue_wait_ms", stats.max_queue_wait_ms}, }; }; + nlohmann::json queues; - queues["color"] = to_json(getColorQueueStats(color_frame_queue_, color_frame_queue_lock_, - color_frame_queue_stats_, - color_frame_queue_max_frames_, request->data)); - queues["left_color"] = to_json(getColorQueueStats( - left_color_frame_queue_, left_color_frame_queue_lock_, left_color_frame_queue_stats_, - left_color_frame_queue_max_frames_, request->data)); - queues["right_color"] = to_json(getColorQueueStats( - right_color_frame_queue_, right_color_frame_queue_lock_, right_color_frame_queue_stats_, - right_color_frame_queue_max_frames_, request->data)); - const uint64_t overflow_count = queues["color"]["overflow_count"].get() + - queues["left_color"]["overflow_count"].get() + - queues["right_color"]["overflow_count"].get(); + uint64_t overflow_count = 0; + if (enable_stream_[COLOR]) { + const auto stats = + getColorQueueStats(color_frame_queue_, color_frame_queue_lock_, color_frame_queue_stats_, + color_frame_queue_max_frames_, request->data); + queues["color"] = to_json(stats); + overflow_count += stats.overflow_count; + } + if (enable_stream_[COLOR_LEFT]) { + const auto stats = getColorQueueStats(left_color_frame_queue_, left_color_frame_queue_lock_, + left_color_frame_queue_stats_, + left_color_frame_queue_max_frames_, request->data); + queues["left_color"] = to_json(stats); + overflow_count += stats.overflow_count; + } + if (enable_stream_[COLOR_RIGHT]) { + const auto stats = getColorQueueStats(right_color_frame_queue_, right_color_frame_queue_lock_, + right_color_frame_queue_stats_, + right_color_frame_queue_max_frames_, request->data); + queues["right_color"] = to_json(stats); + overflow_count += stats.overflow_count; + } response->success = true; response->message = nlohmann::json{ @@ -499,6 +510,9 @@ void OBCameraNode::getColorQueueStatsCallback( {"queues", queues}, } .dump(); + if (queues.empty()) { + RCLCPP_WARN(logger_, "No enabled color streams; color queue statistics are empty"); + } } catch (const std::exception& error) { response->success = false; response->message = error.what(); From e80760c934611917b7e081a8ba0e210b4e1a3600 Mon Sep 17 00:00:00 2001 From: slz Date: Wed, 19 Aug 2026 17:54:49 +0800 Subject: [PATCH 5/6] fix: harden color queue stats and dual-color qos --- .../launch/gemini_301_series.launch.py | 4 +++ orbbec_camera/src/ob_camera_node.cpp | 14 ++++---- orbbec_camera/src/ros_service.cpp | 33 +++++++++++-------- 3 files changed, 31 insertions(+), 20 deletions(-) mode change 100755 => 100644 orbbec_camera/src/ob_camera_node.cpp diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 751f87cb..59ded397 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -124,6 +124,10 @@ def generate_launch_description(): DeclareLaunchArgument('color_qos', default_value='default'), DeclareLaunchArgument('color_qos_history', default_value='default'), DeclareLaunchArgument('color_qos_depth', default_value='-1'), + DeclareLaunchArgument('left_color_qos_history', default_value='default'), + DeclareLaunchArgument('left_color_qos_depth', default_value='-1'), + DeclareLaunchArgument('right_color_qos_history', default_value='default'), + DeclareLaunchArgument('right_color_qos_depth', default_value='-1'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'), DeclareLaunchArgument('color_rotation', default_value='-1'),#color rotation degree : 0, 90, 180, 270 diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp old mode 100755 new mode 100644 index f0c67931..410000a2 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -16,7 +16,6 @@ #include "orbbec_camera/ob_camera_node.h" #include -#include #include #include #include @@ -453,7 +452,8 @@ void OBCameraNode::publishDepthFiltersStatus() { depth_filters_snapshot = depth_filter_list_; } - auto find_depth_filter = [&depth_filters_snapshot](const std::string &filter_name) -> std::shared_ptr { + auto find_depth_filter = + [&depth_filters_snapshot](const std::string &filter_name) -> std::shared_ptr { const auto normalized_name = normalizeDepthFilterName(filter_name); auto it = std::find_if(depth_filters_snapshot.begin(), depth_filters_snapshot.end(), [&normalized_name](const auto &filter) { @@ -5446,14 +5446,14 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { image_publishers_[stream_index] = std::make_shared(*node_, topic, image_qos_profile); } - std::string history = rmw_qos_history_policy_to_str(image_qos_profile.history); + std::string depth; if (image_qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { - history += "(" + std::to_string(image_qos_profile.depth) + ")"; + depth = ", depth=" + std::to_string(image_qos_profile.depth); } RCLCPP_INFO_STREAM( - logger_, topic << " QoS: " << rmw_qos_reliability_policy_to_str(image_qos_profile.reliability) - << ", " << rmw_qos_durability_policy_to_str(image_qos_profile.durability) - << ", " << history); + logger_, topic << " QoS: reliability=" << magic_enum::enum_name(image_qos_profile.reliability) + << ", durability=" << magic_enum::enum_name(image_qos_profile.durability) + << ", history=" << magic_enum::enum_name(image_qos_profile.history) << depth); if (is_mjpg_color_stream) { compressed_image_publishers_[stream_index] = diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 01af1b51..0a73d234 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -480,33 +480,40 @@ void OBCameraNode::getColorQueueStatsCallback( nlohmann::json queues; uint64_t overflow_count = 0; - if (enable_stream_[COLOR]) { + const bool reset = request->data; + if (enable_stream_[COLOR] || reset) { const auto stats = getColorQueueStats(color_frame_queue_, color_frame_queue_lock_, color_frame_queue_stats_, - color_frame_queue_max_frames_, request->data); - queues["color"] = to_json(stats); - overflow_count += stats.overflow_count; + color_frame_queue_max_frames_, reset); + if (enable_stream_[COLOR]) { + queues["color"] = to_json(stats); + overflow_count += stats.overflow_count; + } } - if (enable_stream_[COLOR_LEFT]) { + if (enable_stream_[COLOR_LEFT] || reset) { const auto stats = getColorQueueStats(left_color_frame_queue_, left_color_frame_queue_lock_, left_color_frame_queue_stats_, - left_color_frame_queue_max_frames_, request->data); - queues["left_color"] = to_json(stats); - overflow_count += stats.overflow_count; + left_color_frame_queue_max_frames_, reset); + if (enable_stream_[COLOR_LEFT]) { + queues["left_color"] = to_json(stats); + overflow_count += stats.overflow_count; + } } - if (enable_stream_[COLOR_RIGHT]) { + if (enable_stream_[COLOR_RIGHT] || reset) { const auto stats = getColorQueueStats(right_color_frame_queue_, right_color_frame_queue_lock_, right_color_frame_queue_stats_, - right_color_frame_queue_max_frames_, request->data); - queues["right_color"] = to_json(stats); - overflow_count += stats.overflow_count; + right_color_frame_queue_max_frames_, reset); + if (enable_stream_[COLOR_RIGHT]) { + queues["right_color"] = to_json(stats); + overflow_count += stats.overflow_count; + } } response->success = true; response->message = nlohmann::json{ {"namespace", node_->get_namespace()}, {"overflow_count", overflow_count}, - {"statistics_reset", request->data}, + {"statistics_reset", reset}, {"queues", queues}, } .dump(); From 760634fdab6c9f0c2aac90392eb437a8e6562ba8 Mon Sep 17 00:00:00 2001 From: slz Date: Thu, 20 Aug 2026 14:54:02 +0800 Subject: [PATCH 6/6] refactor: simplify image qos logging --- orbbec_camera/include/orbbec_camera/utils.h | 2 ++ orbbec_camera/src/ob_camera_node.cpp | 10 ++------- orbbec_camera/src/ros_service.cpp | 2 +- orbbec_camera/src/utils.cpp | 24 +++++++++++++++++++++ 4 files changed, 29 insertions(+), 9 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/utils.h b/orbbec_camera/include/orbbec_camera/utils.h index 1f6e6c89..b74ef056 100644 --- a/orbbec_camera/include/orbbec_camera/utils.h +++ b/orbbec_camera/include/orbbec_camera/utils.h @@ -188,6 +188,8 @@ std::string ObDeviceTypeToString(const OBDeviceType& type); rmw_qos_profile_t getRMWQosProfileFromString(const std::string& str_qos); +std::string getRMWQosProfileDescription(const rmw_qos_profile_t& qos_profile); + bool isOpenNIDevice(int pid); OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString( diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 410000a2..97c0dc20 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -5446,14 +5446,8 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { image_publishers_[stream_index] = std::make_shared(*node_, topic, image_qos_profile); } - std::string depth; - if (image_qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { - depth = ", depth=" + std::to_string(image_qos_profile.depth); - } - RCLCPP_INFO_STREAM( - logger_, topic << " QoS: reliability=" << magic_enum::enum_name(image_qos_profile.reliability) - << ", durability=" << magic_enum::enum_name(image_qos_profile.durability) - << ", history=" << magic_enum::enum_name(image_qos_profile.history) << depth); + RCLCPP_INFO_STREAM(logger_, + topic << " QoS: " << getRMWQosProfileDescription(image_qos_profile)); if (is_mjpg_color_stream) { compressed_image_publishers_[stream_index] = diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 0a73d234..0b95972f 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -478,7 +478,7 @@ void OBCameraNode::getColorQueueStatsCallback( }; }; - nlohmann::json queues; + nlohmann::json queues = nlohmann::json::object(); uint64_t overflow_count = 0; const bool reset = request->data; if (enable_stream_[COLOR] || reset) { diff --git a/orbbec_camera/src/utils.cpp b/orbbec_camera/src/utils.cpp index 4f4aef43..819c1f84 100644 --- a/orbbec_camera/src/utils.cpp +++ b/orbbec_camera/src/utils.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "orbbec_camera/utils.h" #include #include "orbbec_camera/constants.h" @@ -566,6 +567,29 @@ rmw_qos_profile_t getRMWQosProfileFromString(const std::string &str_qos) { } } +std::string getRMWQosProfileDescription(const rmw_qos_profile_t &qos_profile) { + const auto short_qos_name = [](auto policy) { + auto name = magic_enum::enum_name(policy); + constexpr size_t prefix_size = sizeof("RMW_QOS_POLICY_") - 1; + if (name.size() <= prefix_size) { + return name; + } + name.remove_prefix(prefix_size); + const auto separator = name.find('_'); + if (separator < name.size()) { + name.remove_prefix(separator + 1); + } + return name; + }; + + std::string history(short_qos_name(qos_profile.history)); + if (qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { + history += "(" + std::to_string(qos_profile.depth) + ")"; + } + return std::string(short_qos_name(qos_profile.reliability)) + "/" + + std::string(short_qos_name(qos_profile.durability)) + "/" + history; +} + bool isOpenNIDevice(int pid) { static const std::vector OPENNI_DEVICE_PIDS = { 0x0300, 0x0301, 0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0407, 0x0601, 0x060b, 0x060e,