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(

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'),
@@ -35,6 +36,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 +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='480'),
@@ -56,6 +61,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'),

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"),
@@ -34,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_ae_max_exposure", default_value="-1"),
@@ -49,6 +52,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 +62,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"),

orbbec_camera/launch/dabai_a.launch.py

@@ -89,12 +89,15 @@ 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'),
         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'),
@@ -117,6 +120,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=''),
@@ -127,6 +132,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'),
@@ -134,6 +141,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'),

orbbec_camera/launch/dabai_al.launch.py

@@ -89,12 +89,15 @@ 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'),
         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'),
@@ -117,6 +120,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=''),
@@ -127,6 +132,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'),
@@ -134,6 +141,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'),

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'),
@@ -35,6 +36,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 +59,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 +71,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'),

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'),
@@ -38,6 +39,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 +66,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 +78,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'),

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"),
@@ -33,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_exposure", default_value="-1"),
@@ -48,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="480"),
@@ -56,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_exposure", default_value="-1"),

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"),
@@ -34,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_ae_max_exposure", default_value="-1"),
@@ -49,6 +52,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 +62,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"),

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"),
@@ -34,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_ae_max_exposure", default_value="-1"),
@@ -49,6 +52,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 +62,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"),

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"),
@@ -34,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_ae_max_exposure", default_value="-1"),
@@ -57,6 +60,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 +74,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"),

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"),
@@ -34,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_ae_max_exposure", default_value="-1"),
@@ -55,6 +58,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 +72,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"),

orbbec_camera/launch/gemini2L.launch.py

@@ -90,6 +90,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"),
@@ -97,6 +98,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"),
@@ -116,6 +119,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"),
@@ -124,6 +129,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"),

orbbec_camera/launch/gemini345.launch.py

@@ -88,12 +88,15 @@ 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'),
         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 +119,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 +131,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 +140,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'),

orbbec_camera/launch/gemini345_lg.launch.py

@@ -89,12 +89,15 @@ 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'),
         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'),
@@ -117,6 +120,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=''),
@@ -127,6 +132,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'),
@@ -134,6 +141,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'),

orbbec_camera/launch/gemini435_le.launch.py

@@ -89,12 +89,15 @@ 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'),
         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
@@ -127,6 +130,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=''),
@@ -144,6 +149,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'),
@@ -154,6 +161,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'),

orbbec_camera/launch/gemini_301_series.launch.py

@@ -112,6 +112,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'),
@@ -120,6 +123,12 @@ 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('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
@@ -156,6 +165,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=''),
@@ -174,6 +185,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'),
@@ -187,6 +200,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'),

orbbec_camera/launch/gemini_330_series.launch.py

@@ -102,12 +102,15 @@ 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'),
         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 +147,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 +166,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 +180,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'),

orbbec_camera/launch/gemini_330_series_low_cpu.launch.py

@@ -90,12 +90,15 @@ 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'),
         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 +135,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 +154,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 +168,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'),

orbbec_camera/launch/gemini_330_series_sdk_json.launch.py

@@ -83,16 +83,34 @@ 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'),
+
+        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'),

orbbec_camera/launch/gemini_intra_process_demo_launch.py

@@ -65,12 +65,15 @@ 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'),
         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'),
@@ -83,6 +86,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'),
@@ -90,6 +95,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'),
@@ -97,6 +104,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'),

orbbec_camera/src/ob_camera_node.cpp

@@ -457,7 +457,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) {
@@ -3821,17 +3822,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;
@@ -3839,6 +3840,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;
@@ -3944,6 +3995,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;
@@ -4387,6 +4444,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";
@@ -4420,6 +4491,20 @@ void OBCameraNode::getParameters() {
     updateImageConfig(stream_index);
     param_name = stream_name_[stream_index] + "_qos";
     setAndGetNodeParameter<std::string>(image_qos_[stream_index], param_name, "default");
+    param_name = stream_name_[stream_index] + "_qos_history";
+    setAndGetNodeParameter<std::string>(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<int>(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<std::string>(camera_info_qos_[stream_index], param_name, "default");
     param_name = "enable_" + stream_name_[stream_index] + "_undistortion";
@@ -5276,10 +5361,7 @@ void OBCameraNode::setupConfidencePublishers() {
   if (confidence_image_publisher_) {
     return;
   }
-  auto image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]);
+  const auto image_qos_profile = getImageQosProfile(DEPTH);
-  if (use_intra_process_) {
-    image_qos_profile = rmw_qos_profile_default;
-  }
   confidence_image_publisher_ = node_->create_publisher<sensor_msgs::msg::Image>(
       "confidence/image_raw",
       rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile));
@@ -5368,11 +5450,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]);
+  const auto image_qos_profile = getImageQosProfile(stream_index);
-  if (use_intra_process_) {
-    image_qos_profile = rmw_qos_profile_default;
-  }
-
   const bool is_mjpg_color_stream =
       (stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
       format_[stream_index] == OB_FORMAT_MJPG;
@@ -5383,6 +5461,8 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
     image_publishers_[stream_index] =
         std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
   }
+  RCLCPP_INFO_STREAM(logger_,
+                     topic << " QoS: " << getRMWQosProfileDescription(image_qos_profile));
 
   if (is_mjpg_color_stream) {
     compressed_image_publishers_[stream_index] =
@@ -5394,6 +5474,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<size_t>(depth);
+  }
+  return image_qos_profile;
+}
+
 void OBCameraNode::setupPublishers() {
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using CameraInfo = sensor_msgs::msg::CameraInfo;
@@ -5544,10 +5642,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]);
+      const auto depth_image_qos_profile = getImageQosProfile(DEPTH);
-      if (use_intra_process_) {
-        depth_image_qos_profile = rmw_qos_profile_default;
-      }
       if (use_intra_process_) {
         depth_unaligned_publisher_ = std::make_shared<image_rcl_publisher>(
             *node_, "depth/image_unaligned", depth_image_qos_profile);
@@ -6371,22 +6466,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_);
+      enqueueColorFrame(color_frame_queue_, color_frame_queue_lock_, color_frame_queue_cv_,
-      color_frame_queue_.push(frame_set);
+                        color_frame_queue_stats_, color_frame_queue_max_frames_, frame_set,
-      color_frame_queue_cv_.notify_all();
+                        "color");
     } else {
       publishPointCloud(frame_set);
     }
 
     if (enable_stream_[COLOR_LEFT] && left_color_frame) {
-      std::unique_lock<std::mutex> lock(left_color_frame_queue_lock_);
+      enqueueColorFrame(left_color_frame_queue_, left_color_frame_queue_lock_,
-      left_color_frame_queue_.push(frame_set);
+                        left_color_frame_queue_cv_, left_color_frame_queue_stats_,
-      left_color_frame_queue_cv_.notify_all();
+                        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_);
+      enqueueColorFrame(right_color_frame_queue_, right_color_frame_queue_lock_,
-      right_color_frame_queue_.push(frame_set);
+                        right_color_frame_queue_cv_, right_color_frame_queue_stats_,
-      right_color_frame_queue_cv_.notify_all();
+                        right_color_frame_queue_max_frames_, frame_set, "right_color");
     }
 
     for (const auto &stream_index : IMAGE_STREAMS) {
@@ -6438,6 +6533,8 @@ 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::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()) ||
@@ -6447,11 +6544,19 @@ void OBCameraNode::onNewColorFrameCallback() {
       if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
         break;
       }
-    std::shared_ptr<ob::FrameSet> frameSet = color_frame_queue_.front();
+      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());
+    }
+
     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");
@@ -6460,6 +6565,8 @@ void OBCameraNode::onNewColorFrameCallback() {
 void OBCameraNode::onNewLeftColorFrameCallback() {
   while (enable_stream_[COLOR_LEFT] && rclcpp::ok() && 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()) ||
@@ -6469,11 +6576,19 @@ void OBCameraNode::onNewLeftColorFrameCallback() {
       if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
         break;
       }
-    std::shared_ptr<ob::FrameSet> frameSet = left_color_frame_queue_.front();
+      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());
+    }
+
     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");
 }
@@ -6481,6 +6596,8 @@ void OBCameraNode::onNewLeftColorFrameCallback() {
 void OBCameraNode::onNewRightColorFrameCallback() {
   while (enable_stream_[COLOR_RIGHT] && rclcpp::ok() && 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()) ||
@@ -6490,11 +6607,19 @@ void OBCameraNode::onNewRightColorFrameCallback() {
       if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
         break;
       }
-    std::shared_ptr<ob::FrameSet> frameSet = right_color_frame_queue_.front();
+      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());
+    }
+
     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,69 @@ 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 = nlohmann::json::object();
+    uint64_t overflow_count = 0;
+    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_, reset);
+      if (enable_stream_[COLOR]) {
+        queues["color"] = to_json(stats);
+        overflow_count += stats.overflow_count;
+      }
+    }
+    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_, reset);
+      if (enable_stream_[COLOR_LEFT]) {
+        queues["left_color"] = to_json(stats);
+        overflow_count += stats.overflow_count;
+      }
+    }
+    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_, 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", reset},
+            {"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();
+  }
+}
+
 void OBCameraNode::getPointCloudDecimationCallback(
     const std::shared_ptr<GetInt32::Request>& request,
     std::shared_ptr<GetInt32::Response>& response) {

orbbec_camera/src/utils.cpp

@@ -23,6 +23,7 @@
 #include <regex>
 #include <sstream>
 #include <vector>
+#include <magic_enum/magic_enum.hpp>
 #include "orbbec_camera/utils.h"
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #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<int> OPENNI_DEVICE_PIDS = {
       0x0300, 0x0301, 0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0407, 0x0601, 0x060b, 0x060e,