i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-09 22:29:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: monitor side camera streams

Browse Source
This commit is contained in:
slz
2026-09-10 10:45:25 +08:00
parent b76e062fe5
commit a3a7231302
9 changed files with 290 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/fps_delay_status.hpp
+45 -26
View File
@@ -20,6 +20,7 @@
#include "orbbec_camera_msgs/msg/device_status.hpp"
#include <chrono>
#include <functional>
#include <limits>
#include <mutex>
namespace orbbec_camera {
class FpsDelayStatus {
@@ -58,38 +59,57 @@ class FpsDelayStatus {
}
void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
std::lock_guard<std::mutex> lock(mutex_);
msg.color_frame_rate_cur = last_fps_;
msg.color_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
msg.color_frame_rate_min = fps_min_;
msg.color_frame_rate_max = fps_max_;
fillStatus(msg.color_frame_rate_cur, msg.color_frame_rate_avg, msg.color_frame_rate_min,
msg.color_frame_rate_max, msg.color_delay_ms_cur, msg.color_delay_ms_avg,
msg.color_delay_ms_min, msg.color_delay_ms_max);
}
msg.color_delay_ms_cur = last_delay_ms_;
msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
msg.color_delay_ms_min = delay_min_;
msg.color_delay_ms_max = delay_max_;
void fillLeftColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.left_color_frame_rate_cur, msg.left_color_frame_rate_avg,
msg.left_color_frame_rate_min, msg.left_color_frame_rate_max,
msg.left_color_delay_ms_cur, msg.left_color_delay_ms_avg,
msg.left_color_delay_ms_min, msg.left_color_delay_ms_max);
}
last_delay_ms_ = 0.0;
last_fps_ = 0.0;
frame_count_ = 0;
fps_sum_ = delay_sum_ = 0.0;
fps_max_ = delay_max_ = 0.0;
fps_min_ = delay_min_ = 0.0;
void fillRightColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.right_color_frame_rate_cur, msg.right_color_frame_rate_avg,
msg.right_color_frame_rate_min, msg.right_color_frame_rate_max,
msg.right_color_delay_ms_cur, msg.right_color_delay_ms_avg,
msg.right_color_delay_ms_min, msg.right_color_delay_ms_max);
}
void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.depth_frame_rate_cur, msg.depth_frame_rate_avg, msg.depth_frame_rate_min,
msg.depth_frame_rate_max, msg.depth_delay_ms_cur, msg.depth_delay_ms_avg,
msg.depth_delay_ms_min, msg.depth_delay_ms_max);
}
void fillLeftIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.left_ir_frame_rate_cur, msg.left_ir_frame_rate_avg, msg.left_ir_frame_rate_min,
msg.left_ir_frame_rate_max, msg.left_ir_delay_ms_cur, msg.left_ir_delay_ms_avg,
msg.left_ir_delay_ms_min, msg.left_ir_delay_ms_max);
}
void fillRightIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.right_ir_frame_rate_cur, msg.right_ir_frame_rate_avg,
msg.right_ir_frame_rate_min, msg.right_ir_frame_rate_max, msg.right_ir_delay_ms_cur,
msg.right_ir_delay_ms_avg, msg.right_ir_delay_ms_min, msg.right_ir_delay_ms_max);
}
private:
void fillStatus(double &frame_rate_cur, double &frame_rate_avg, double &frame_rate_min,
double &frame_rate_max, double &delay_ms_cur, double &delay_ms_avg,
double &delay_ms_min, double &delay_ms_max) {
std::lock_guard<std::mutex> lock(mutex_);
msg.depth_frame_rate_cur = last_fps_;
msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
msg.depth_frame_rate_min = fps_min_;
msg.depth_frame_rate_max = fps_max_;
frame_rate_cur = last_fps_;
frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
frame_rate_min = frame_count_ > 0 ? fps_min_ : 0;
frame_rate_max = frame_count_ > 0 ? fps_max_ : 0;
msg.depth_delay_ms_cur = last_delay_ms_;
msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
msg.depth_delay_ms_min = delay_min_;
msg.depth_delay_ms_max = delay_max_;
// RCLCPP_ERROR_STREAM(logger_, "Depth status: " << fps_sum_ << "," << frame_count_);
delay_ms_cur = last_delay_ms_;
delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
delay_ms_min = frame_count_ > 0 ? delay_min_ : 0;
delay_ms_max = frame_count_ > 0 ? delay_max_ : 0;
last_delay_ms_ = 0.0;
last_fps_ = 0.0;
@@ -99,7 +119,6 @@ class FpsDelayStatus {
fps_min_ = delay_min_ = 0.0;
}
private:
mutable std::mutex mutex_;
u_int64_t last_stream_timestamp_{0};
double last_delay_ms_{0.0};
orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h
+1 -1
View File
@@ -21,7 +21,7 @@ namespace orbbec_camera {
class FrameTimestampCsvLogger {
public:
enum class OutputMode { SYNCED, COLOR, DEPTH };
enum class OutputMode { SYNCED, COLOR, LEFT_COLOR, RIGHT_COLOR, DEPTH, LEFT_IR, RIGHT_IR };
FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
OutputMode output_mode, rclcpp::Logger logger);
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+10
View File
@@ -237,10 +237,14 @@ class OBCameraNode {
}
void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
fps_delay_status_color_->fillColorStatus(status_msg);
fps_delay_status_left_color_->fillLeftColorStatus(status_msg);
fps_delay_status_right_color_->fillRightColorStatus(status_msg);
}
void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
fps_delay_status_depth_->fillDepthStatus(status_msg);
fps_delay_status_left_ir_->fillLeftIrStatus(status_msg);
fps_delay_status_right_ir_->fillRightIrStatus(status_msg);
}
bool checkUserCalibrationReady() {
@@ -1188,12 +1192,18 @@ class OBCameraNode {
bool show_fps_enable_ = false;
bool enable_publish_extrinsic_ = false;
std::unique_ptr<FpsCounter> fps_counter_color_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_left_color_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_right_color_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_depth_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_right_ir_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_left_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_right_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_depth_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_left_ir_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_right_ir_{nullptr};
std::string intra_camera_sync_reference_ = "";
std::string ae_reference_stream_;
orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h
+11
View File
@@ -22,7 +22,11 @@ class TimestampCsvLogger {
std::string csv_file_path;
bool frame_sync_enabled = false;
bool color_enabled = false;
bool left_color_enabled = false;
bool right_color_enabled = false;
bool depth_enabled = false;
bool left_ir_enabled = false;
bool right_ir_enabled = false;
bool imu_sync_enabled = false;
bool accel_enabled = false;
bool gyro_enabled = false;
@@ -44,6 +48,9 @@ class TimestampCsvLogger {
const std::shared_ptr<ob::Frame> &depth_frame, int64_t arrival_system_us,
int64_t arrival_steady_us, bool track_color, bool track_depth,
bool color_image_publish_expected, bool depth_image_publish_expected);
void recordImageFrameArrival(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
int64_t arrival_system_us, int64_t arrival_steady_us,
bool image_publish_expected);
void recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us, int64_t publish_steady_us);
void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame);
@@ -64,7 +71,11 @@ class TimestampCsvLogger {
std::atomic_bool shutdown_requested_{false};
std::unique_ptr<FrameTimestampCsvLogger> synced_image_logger_;
std::unique_ptr<FrameTimestampCsvLogger> color_logger_;
std::unique_ptr<FrameTimestampCsvLogger> left_color_logger_;
std::unique_ptr<FrameTimestampCsvLogger> right_color_logger_;
std::unique_ptr<FrameTimestampCsvLogger> depth_logger_;
std::unique_ptr<FrameTimestampCsvLogger> left_ir_logger_;
std::unique_ptr<FrameTimestampCsvLogger> right_ir_logger_;
std::unique_ptr<ImuTimestampCsvLogger> synced_imu_logger_;
std::unique_ptr<ImuTimestampCsvLogger> accel_logger_;
std::unique_ptr<ImuTimestampCsvLogger> gyro_logger_;