i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 20:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: bound and diagnose color frame queues

Browse Source
This commit is contained in:
slz
2026-08-19 10:27:58 +08:00
parent ac4346468e
commit c141430c62
23 changed files with 244 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+41 -3
View File
@@ -18,9 +18,12 @@
#include <nlohmann/json.hpp>
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <memory>
#include <optional>
#include <queue>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <unordered_map>
@@ -334,6 +337,9 @@ class OBCameraNode {
void setupCameraCtrlServices();
void getColorQueueStatsCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void stopStreams();
void stopIMU();
@@ -772,6 +778,7 @@ class OBCameraNode {
rclcpp::Service<SetInt32>::SharedPtr set_sync_io_voltage_level_srv_;
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_streams_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_streams_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr get_color_queue_stats_srv_;
rclcpp::Service<SetString>::SharedPtr set_image_registration_mode_srv_;
rclcpp::Service<SetStreamProfile>::SharedPtr set_stream_profile_srv_;
rclcpp::Service<GetUserCalibParams>::SharedPtr get_user_calib_params_srv_;
@@ -915,21 +922,52 @@ class OBCameraNode {
bool is_right_color_frame_decoded_ = false;
bool is_color_frame_decoded_ = false;
std::recursive_mutex device_lock_;
struct QueuedColorFrame {
std::shared_ptr<ob::FrameSet> frame_set;
std::chrono::steady_clock::time_point enqueue_time;
};
struct ColorQueueStats {
size_t max_queue_size = 0;
uint64_t overflow_count = 0;
double max_queue_wait_ms = 0.0;
};
struct ColorQueueStatsSnapshot {
int capacity_frames = 0;
size_t queue_size = 0;
size_t max_queue_size = 0;
uint64_t overflow_count = 0;
double oldest_queue_wait_ms = 0.0;
double max_queue_wait_ms = 0.0;
};
using ColorFrameQueue = std::queue<QueuedColorFrame>;
void enqueueColorFrame(ColorFrameQueue& queue, std::mutex& mutex,
std::condition_variable& condition_variable, ColorQueueStats& stats,
int capacity_frames, const std::shared_ptr<ob::FrameSet>& frame_set,
const char* queue_name);
ColorQueueStatsSnapshot getColorQueueStats(ColorFrameQueue& queue, std::mutex& mutex,
ColorQueueStats& stats, int capacity_frames,
bool reset);
// For color
std::queue<std::shared_ptr<ob::FrameSet>> color_frame_queue_;
ColorFrameQueue color_frame_queue_;
ColorQueueStats color_frame_queue_stats_;
int color_frame_queue_max_frames_ = 10;
std::shared_ptr<std::thread> colorFrameThread_ = nullptr;
std::atomic_bool stop_color_frame_threads_{false};
std::mutex color_frame_queue_lock_;
std::condition_variable color_frame_queue_cv_;
// For left color
std::queue<std::shared_ptr<ob::FrameSet>> left_color_frame_queue_;
ColorFrameQueue left_color_frame_queue_;
ColorQueueStats left_color_frame_queue_stats_;
int left_color_frame_queue_max_frames_ = 10;
std::shared_ptr<std::thread> leftColorFrameThread_ = nullptr;
std::mutex left_color_frame_queue_lock_;
std::condition_variable left_color_frame_queue_cv_;
// For right color
std::queue<std::shared_ptr<ob::FrameSet>> right_color_frame_queue_;
ColorFrameQueue right_color_frame_queue_;
ColorQueueStats right_color_frame_queue_stats_;
int right_color_frame_queue_max_frames_ = 10;
std::shared_ptr<std::thread> rightColorFrameThread_ = nullptr;
std::mutex right_color_frame_queue_lock_;
std::condition_variable right_color_frame_queue_cv_;