fix: harden color queue stats and dual-color qos

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

orbbec_camera/src/ob_camera_node.cpp

@@ -16,7 +16,6 @@
 
 #include "orbbec_camera/ob_camera_node.h"
 #include <rclcpp/rclcpp.hpp>
-#include <rmw/qos_string_conversions.h>
 #include <thread>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <sstream>
@@ -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<ob::Filter> {
+  auto find_depth_filter =
+      [&depth_filters_snapshot](const std::string &filter_name) -> std::shared_ptr<ob::Filter> {
     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<image_transport_publisher>(*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)
+      logger_, topic << " QoS: reliability=" << magic_enum::enum_name(image_qos_profile.reliability)
-                     << ", " << rmw_qos_durability_policy_to_str(image_qos_profile.durability)
+                     << ", durability=" << magic_enum::enum_name(image_qos_profile.durability)
-                     << ", " << history);
+                     << ", history=" << magic_enum::enum_name(image_qos_profile.history) << depth);
 
   if (is_mjpg_color_stream) {
     compressed_image_publishers_[stream_index] =

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);
+                             color_frame_queue_max_frames_, reset);
-      queues["color"] = to_json(stats);
+      if (enable_stream_[COLOR]) {
-      overflow_count += stats.overflow_count;
+        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);
+                                            left_color_frame_queue_max_frames_, reset);
-      queues["left_color"] = to_json(stats);
+      if (enable_stream_[COLOR_LEFT]) {
-      overflow_count += stats.overflow_count;
+        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);
+                                            right_color_frame_queue_max_frames_, reset);
-      queues["right_color"] = to_json(stats);
+      if (enable_stream_[COLOR_RIGHT]) {
-      overflow_count += stats.overflow_count;
+        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();