From c141430c62e22d2b28cf564d211a56b3c2837c21 Mon Sep 17 00:00:00 2001 From: slz Date: Wed, 19 Aug 2026 10:27:58 +0800 Subject: [PATCH] feat: bound and diagnose color frame queues --- .../include/orbbec_camera/ob_camera_node.h | 44 ++++- orbbec_camera/launch/astra.launch.py | 1 + orbbec_camera/launch/astra2.launch.py | 1 + orbbec_camera/launch/dabai_a.launch.py | 1 + orbbec_camera/launch/dabai_al.launch.py | 1 + orbbec_camera/launch/dabai_dcw2.launch.py | 1 + orbbec_camera/launch/dabai_max_pro.launch.py | 1 + orbbec_camera/launch/femto.launch.py | 1 + orbbec_camera/launch/femto_bolt.launch.py | 1 + orbbec_camera/launch/femto_mega.launch.py | 1 + orbbec_camera/launch/gemini2.launch.py | 1 + orbbec_camera/launch/gemini210.launch.py | 1 + orbbec_camera/launch/gemini2L.launch.py | 1 + orbbec_camera/launch/gemini345.launch.py | 1 + orbbec_camera/launch/gemini345_lg.launch.py | 1 + orbbec_camera/launch/gemini435_le.launch.py | 1 + .../launch/gemini_301_series.launch.py | 3 + .../launch/gemini_330_series.launch.py | 1 + .../gemini_330_series_low_cpu.launch.py | 1 + .../gemini_330_series_sdk_json.launch.py | 2 + .../gemini_intra_process_demo_launch.py | 1 + orbbec_camera/src/ob_camera_node.cpp | 172 ++++++++++++++---- orbbec_camera/src/ros_service.cpp | 47 +++++ 23 files changed, 244 insertions(+), 42 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index ec59c93a..d80a6f18 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -18,9 +18,12 @@ #include +#include #include +#include #include #include +#include #include #include #include @@ -334,6 +337,9 @@ class OBCameraNode { void setupCameraCtrlServices(); + void getColorQueueStatsCallback(const std::shared_ptr& request, + std::shared_ptr& response); + void stopStreams(); void stopIMU(); @@ -772,6 +778,7 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_sync_io_voltage_level_srv_; rclcpp::Service::SharedPtr get_streams_enable_srv_; rclcpp::Service::SharedPtr set_streams_enable_srv_; + rclcpp::Service::SharedPtr get_color_queue_stats_srv_; rclcpp::Service::SharedPtr set_image_registration_mode_srv_; rclcpp::Service::SharedPtr set_stream_profile_srv_; rclcpp::Service::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 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; + void enqueueColorFrame(ColorFrameQueue& queue, std::mutex& mutex, + std::condition_variable& condition_variable, ColorQueueStats& stats, + int capacity_frames, const std::shared_ptr& frame_set, + const char* queue_name); + ColorQueueStatsSnapshot getColorQueueStats(ColorFrameQueue& queue, std::mutex& mutex, + ColorQueueStats& stats, int capacity_frames, + bool reset); // For color - std::queue> color_frame_queue_; + ColorFrameQueue color_frame_queue_; + ColorQueueStats color_frame_queue_stats_; + int color_frame_queue_max_frames_ = 10; std::shared_ptr 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> 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 leftColorFrameThread_ = nullptr; std::mutex left_color_frame_queue_lock_; std::condition_variable left_color_frame_queue_cv_; // For right color - std::queue> 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 rightColorFrameThread_ = nullptr; std::mutex right_color_frame_queue_lock_; std::condition_variable right_color_frame_queue_cv_; diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index 3c1e838b..8921c357 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index 7a97a4e7..8dc5e316 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/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"), diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index 4d7b904e..2151043d 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index 5640afc6..42f708b1 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py index ddd0c11f..2bdf6716 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/dabai_max_pro.launch.py b/orbbec_camera/launch/dabai_max_pro.launch.py index 1e239305..7abbbe84 100644 --- a/orbbec_camera/launch/dabai_max_pro.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 2da1beac..096de049 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/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"), diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index cd3cb0b9..ad857ee6 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/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"), diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 7a643aa3..b882d16b 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/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"), diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index a64dd3f8..21c5f8e3 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/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"), diff --git a/orbbec_camera/launch/gemini210.launch.py b/orbbec_camera/launch/gemini210.launch.py index a48c9773..23bda161 100644 --- a/orbbec_camera/launch/gemini210.launch.py +++ b/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"), diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index bfec3b99..8dec1b3b 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/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"), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index 29efb1ad..d59befc0 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index a5ec7a15..947c416f 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py index 2eca0b27..d2ad1764 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 23e57da5..751f87cb 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index bcce78e2..edbdfb75 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index 7089840e..922d9974 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py index 7a8e862e..b3e65f3d 100644 --- a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py +++ b/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'), diff --git a/orbbec_camera/launch/gemini_intra_process_demo_launch.py b/orbbec_camera/launch/gemini_intra_process_demo_launch.py index 43ec8282..1ecf1b7f 100644 --- a/orbbec_camera/launch/gemini_intra_process_demo_launch.py +++ b/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'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 925726f5..f0c67931 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -3811,17 +3811,17 @@ bool OBCameraNode::applyStreamProfiles(const std::vector & void OBCameraNode::clearColorFrameQueues() { { std::lock_guard lock(color_frame_queue_lock_); - std::queue> empty; + ColorFrameQueue empty; std::swap(color_frame_queue_, empty); } { std::lock_guard lock(left_color_frame_queue_lock_); - std::queue> empty; + ColorFrameQueue empty; std::swap(left_color_frame_queue_, empty); } { std::lock_guard lock(right_color_frame_queue_lock_); - std::queue> 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 &frame_set, + const char *queue_name) { + uint64_t overflow_count = 0; + { + std::lock_guard lock(mutex); + const auto now = std::chrono::steady_clock::now(); + if (queue.size() >= static_cast(capacity_frames)) { + const auto oldest_age = + std::chrono::duration(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 lock(mutex); + const auto oldest_queue_wait_ms = + queue.empty() ? 0.0 + : std::chrono::duration(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(camera_name_, "camera_name", "camera"); + setAndGetNodeParameter(color_frame_queue_max_frames_, "color_frame_queue_max_frames", 10); + setAndGetNodeParameter(left_color_frame_queue_max_frames_, + "left_color_frame_queue_max_frames", 10); + setAndGetNodeParameter(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 frame_set } if (enable_stream_[COLOR] && color_frame) { - std::unique_lock 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 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 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 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 frameSet; + { + std::unique_lock 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(std::chrono::steady_clock::now() - + queued.enqueue_time) + .count()); } - std::shared_ptr 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 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 frameSet; + { + std::unique_lock 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(std::chrono::steady_clock::now() - + queued.enqueue_time) + .count()); } - std::shared_ptr 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 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 frameSet; + { + std::unique_lock 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(std::chrono::steady_clock::now() - + queued.enqueue_time) + .count()); } - std::shared_ptr 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"); } diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 8c4be54b..5a8a3fbf 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/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( + "get_color_queue_stats", [this](const std::shared_ptr request, + std::shared_ptr 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& request, + std::shared_ptr& 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() + + queues["left_color"]["overflow_count"].get() + + queues["right_color"]["overflow_count"].get(); + 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& request, std::shared_ptr& response) {