From e80760c934611917b7e081a8ba0e210b4e1a3600 Mon Sep 17 00:00:00 2001 From: slz Date: Wed, 19 Aug 2026 17:54:49 +0800 Subject: [PATCH] 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();