feat: bound and diagnose color frame queues

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -18,9 +18,12 @@
 
 #include <nlohmann/json.hpp>
 
+#include <chrono>
 #include <cstddef>
+#include <cstdint>
 #include <memory>
 #include <optional>
+#include <queue>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 #include <unordered_map>
@@ -334,6 +337,9 @@ class OBCameraNode {
 
   void setupCameraCtrlServices();
 
+  void getColorQueueStatsCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+                                  std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
+
   void stopStreams();
 
   void stopIMU();
@@ -772,6 +778,7 @@ class OBCameraNode {
   rclcpp::Service<SetInt32>::SharedPtr set_sync_io_voltage_level_srv_;
   rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_streams_enable_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_streams_enable_srv_;
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr get_color_queue_stats_srv_;
   rclcpp::Service<SetString>::SharedPtr set_image_registration_mode_srv_;
   rclcpp::Service<SetStreamProfile>::SharedPtr set_stream_profile_srv_;
   rclcpp::Service<GetUserCalibParams>::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<ob::FrameSet> 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<QueuedColorFrame>;
+  void enqueueColorFrame(ColorFrameQueue& queue, std::mutex& mutex,
+                         std::condition_variable& condition_variable, ColorQueueStats& stats,
+                         int capacity_frames, const std::shared_ptr<ob::FrameSet>& frame_set,
+                         const char* queue_name);
+  ColorQueueStatsSnapshot getColorQueueStats(ColorFrameQueue& queue, std::mutex& mutex,
+                                             ColorQueueStats& stats, int capacity_frames,
+                                             bool reset);
   // For color
-  std::queue<std::shared_ptr<ob::FrameSet>> color_frame_queue_;
+  ColorFrameQueue color_frame_queue_;
+  ColorQueueStats color_frame_queue_stats_;
+  int color_frame_queue_max_frames_ = 10;
   std::shared_ptr<std::thread> 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<std::shared_ptr<ob::FrameSet>> 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<std::thread> leftColorFrameThread_ = nullptr;
   std::mutex left_color_frame_queue_lock_;
   std::condition_variable left_color_frame_queue_cv_;
 
   // For right color
-  std::queue<std::shared_ptr<ob::FrameSet>> 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<std::thread> rightColorFrameThread_ = nullptr;
   std::mutex right_color_frame_queue_lock_;
   std::condition_variable right_color_frame_queue_cv_;

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'),

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"),

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'),

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'),

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'),

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'),

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"),

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"),

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"),

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"),

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"),

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"),

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'),

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'),

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'),

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'),

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'),

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'),

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'),

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'),

orbbec_camera/src/ob_camera_node.cpp

@@ -3811,17 +3811,17 @@ bool OBCameraNode::applyStreamProfiles(const std::vector<PendingStreamProfile> &
 void OBCameraNode::clearColorFrameQueues() {
   {
     std::lock_guard<std::mutex> lock(color_frame_queue_lock_);
-    std::queue<std::shared_ptr<ob::FrameSet>> empty;
+    ColorFrameQueue empty;
     std::swap(color_frame_queue_, empty);
   }
   {
     std::lock_guard<std::mutex> lock(left_color_frame_queue_lock_);
-    std::queue<std::shared_ptr<ob::FrameSet>> empty;
+    ColorFrameQueue empty;
     std::swap(left_color_frame_queue_, empty);
   }
   {
     std::lock_guard<std::mutex> lock(right_color_frame_queue_lock_);
-    std::queue<std::shared_ptr<ob::FrameSet>> 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<ob::FrameSet> &frame_set,
+                                     const char *queue_name) {
+  uint64_t overflow_count = 0;
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    const auto now = std::chrono::steady_clock::now();
+    if (queue.size() >= static_cast<size_t>(capacity_frames)) {
+      const auto oldest_age =
+          std::chrono::duration<double, std::milli>(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<std::mutex> lock(mutex);
+  const auto oldest_queue_wait_ms =
+      queue.empty() ? 0.0
+                    : std::chrono::duration<double, std::milli>(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<std::string>(camera_name_, "camera_name", "camera");
+  setAndGetNodeParameter<int>(color_frame_queue_max_frames_, "color_frame_queue_max_frames", 10);
+  setAndGetNodeParameter<int>(left_color_frame_queue_max_frames_,
+                              "left_color_frame_queue_max_frames", 10);
+  setAndGetNodeParameter<int>(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<ob::FrameSet> frame_set
     }
 
     if (enable_stream_[COLOR] && color_frame) {
-      std::unique_lock<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<ob::FrameSet> frameSet;
+    {
+      std::unique_lock<std::mutex> 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<double, std::milli>(std::chrono::steady_clock::now() -
+                                                             queued.enqueue_time)
+                       .count());
     }
-    std::shared_ptr<ob::FrameSet> 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<std::mutex> 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<ob::FrameSet> frameSet;
+    {
+      std::unique_lock<std::mutex> 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<double, std::milli>(std::chrono::steady_clock::now() -
+                                                             queued.enqueue_time)
+                       .count());
     }
-    std::shared_ptr<ob::FrameSet> 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<std::mutex> 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<ob::FrameSet> frameSet;
+    {
+      std::unique_lock<std::mutex> 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<double, std::milli>(std::chrono::steady_clock::now() -
+                                                             queued.enqueue_time)
+                       .count());
     }
-    std::shared_ptr<ob::FrameSet> 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");
 }

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<SetBool>(
+      "get_color_queue_stats", [this](const std::shared_ptr<SetBool::Request> request,
+                                      std::shared_ptr<SetBool::Response> 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<std_srvs::srv::SetBool::Request>& request,
+    std::shared_ptr<std_srvs::srv::SetBool::Response>& 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<uint64_t>() +
+                                    queues["left_color"]["overflow_count"].get<uint64_t>() +
+                                    queues["right_color"]["overflow_count"].get<uint64_t>();
+    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<GetInt32::Request>& request,
     std::shared_ptr<GetInt32::Response>& response) {