Merge remote-tracking branch 'origin/feat/image-qos-color-queue' into v2/develop

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>
@@ -324,6 +327,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();
@@ -332,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();
@@ -697,6 +705,8 @@ class OBCameraNode {
   std::string camera_link_frame_id_;
   bool depth_registration_ = false;
   std::map<stream_index_pair, std::string> image_qos_;
+  std::map<stream_index_pair, std::string> image_qos_history_;
+  std::map<stream_index_pair, int> image_qos_depth_;
   std::map<stream_index_pair, std::string> camera_info_qos_;
   std::map<stream_index_pair, ob_format> format_;
   std::map<stream_index_pair, std::string> format_str_;
@@ -773,6 +783,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_;
@@ -918,21 +929,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/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(