i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-09 14:27:02 +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/src/frame_timestamp_csv_logger.cpp
+67 -26
View File
@@ -31,6 +31,47 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
}
std::optional<FrameTimestampCsvLogger::OutputMode> outputModeForStream(OBStreamType stream_type) {
using OutputMode = FrameTimestampCsvLogger::OutputMode;
switch (stream_type) {
case OB_STREAM_COLOR:
return OutputMode::COLOR;
case OB_STREAM_COLOR_LEFT:
return OutputMode::LEFT_COLOR;
case OB_STREAM_COLOR_RIGHT:
return OutputMode::RIGHT_COLOR;
case OB_STREAM_DEPTH:
return OutputMode::DEPTH;
case OB_STREAM_IR_LEFT:
return OutputMode::LEFT_IR;
case OB_STREAM_IR_RIGHT:
return OutputMode::RIGHT_IR;
default:
return std::nullopt;
}
}
const char *outputModeName(FrameTimestampCsvLogger::OutputMode output_mode) {
using OutputMode = FrameTimestampCsvLogger::OutputMode;
switch (output_mode) {
case OutputMode::COLOR:
return "color";
case OutputMode::LEFT_COLOR:
return "left_color";
case OutputMode::RIGHT_COLOR:
return "right_color";
case OutputMode::DEPTH:
return "depth";
case OutputMode::LEFT_IR:
return "left_ir";
case OutputMode::RIGHT_IR:
return "right_ir";
case OutputMode::SYNCED:
return "synced";
}
return "unknown";
}
} // namespace
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
@@ -104,9 +145,8 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t
int64_t arrival_system_us,
int64_t arrival_steady_us,
bool image_publish_expected) {
if (!enabled_ || !frame || !isTrackedStream(stream_type) ||
(stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) ||
(stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) {
const auto expected_output_mode = outputModeForStream(stream_type);
if (!enabled_ || !frame || !expected_output_mode || output_mode_ != *expected_output_mode) {
return;
}
recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -163,14 +203,11 @@ void FrameTimestampCsvLogger::shutdown() {
FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream(
OBStreamType stream_type) const {
if (stream_type == OB_STREAM_COLOR) {
return TrackedStream::COLOR;
}
return TrackedStream::DEPTH;
return stream_type == OB_STREAM_DEPTH ? TrackedStream::DEPTH : TrackedStream::COLOR;
}
bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const {
return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH;
return outputModeForStream(stream_type).has_value();
}
void FrameTimestampCsvLogger::recordFrameSetInternal(
@@ -294,10 +331,12 @@ void FrameTimestampCsvLogger::completeImagePublishInternal(
auto row_id_it = row_map.find(frame_index);
if (row_id_it == row_map.end()) {
if (publish_system_us.has_value()) {
RCLCPP_WARN_STREAM(logger_,
"Frame timestamp CSV logger missed row mapping for stream "
<< (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
<< " frame index " << frame_index);
RCLCPP_WARN_STREAM(
logger_, "Frame timestamp CSV logger missed row mapping for stream "
<< (output_mode_ == OutputMode::SYNCED
? (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
: outputModeName(output_mode_))
<< " frame index " << frame_index);
}
return;
}
@@ -357,7 +396,10 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
previous.dropped_frames += lost_frames;
RCLCPP_WARN_STREAM(logger_,
"Frame drop detected: stage=SDK_RECEIVE"
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
<< " stream="
<< (output_mode_ == OutputMode::SYNCED
? (stream == TrackedStream::COLOR ? "color" : "depth")
: outputModeName(output_mode_))
<< " frame_index=" << state.frame_index
<< " dropped=" << previous.dropped_frames);
}
@@ -408,7 +450,10 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
previous.publish_dropped_frames += lost_frames;
RCLCPP_WARN_STREAM(logger_,
"Frame drop detected: stage=ROS_PUBLISH"
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
<< " stream="
<< (output_mode_ == OutputMode::SYNCED
? (stream == TrackedStream::COLOR ? "color" : "depth")
: outputModeName(output_mode_))
<< " frame_index=" << state.frame_index
<< " dropped=" << previous.publish_dropped_frames);
}
@@ -485,12 +530,12 @@ void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row
}
std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
if (output_mode_ == OutputMode::COLOR) {
return serializeStreamColumns(row.color);
}
if (output_mode_ == OutputMode::DEPTH) {
return serializeStreamColumns(row.depth);
}
if (output_mode_ != OutputMode::SYNCED) {
return serializeStreamColumns(row.color);
}
std::ostringstream ss;
ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
return ss.str();
@@ -560,14 +605,12 @@ std::string FrameTimestampCsvLogger::csvHeader() const {
ss << prefix << "_sdk_delay_from_global_us,";
ss << prefix << "_sdk_delay_from_system_us";
};
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) {
append_stream_header("color");
}
if (output_mode_ == OutputMode::SYNCED) {
append_stream_header("color");
ss << ",";
}
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
append_stream_header("depth");
} else {
append_stream_header(outputModeName(output_mode_));
}
return ss.str();
}
@@ -641,10 +684,8 @@ void FrameTimestampCsvLogger::writerThreadMain() {
std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
const std::filesystem::path original_path(csv_file_path_);
std::string suffix;
if (output_mode_ == OutputMode::COLOR) {
suffix = "_color";
} else if (output_mode_ == OutputMode::DEPTH) {
suffix = "_depth";
if (output_mode_ != OutputMode::SYNCED) {
suffix = "_" + std::string(outputModeName(output_mode_));
}
auto indexed_filename = original_path.stem().string() + suffix;
orbbec_camera/src/ob_camera_node.cpp
+47 -3
View File
@@ -741,7 +741,11 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
timestamp_config.csv_file_path = frame_timestamp_csv_file_;
timestamp_config.frame_sync_enabled = enable_frame_sync_;
timestamp_config.color_enabled = enable_stream_[COLOR];
timestamp_config.left_color_enabled = enable_stream_[COLOR_LEFT];
timestamp_config.right_color_enabled = enable_stream_[COLOR_RIGHT];
timestamp_config.depth_enabled = enable_stream_[DEPTH];
timestamp_config.left_ir_enabled = enable_stream_[INFRA1];
timestamp_config.right_ir_enabled = enable_stream_[INFRA2];
timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
timestamp_config.accel_enabled = enable_stream_[ACCEL];
timestamp_config.gyro_enabled = enable_stream_[GYRO];
@@ -761,6 +765,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
is_camera_node_initialized_ = true;
fps_counter_color_ = std::make_unique<FpsCounter>("Color", logger_, 1);
fps_counter_left_color_ = std::make_unique<FpsCounter>("Left Color", logger_, 1);
fps_counter_right_color_ = std::make_unique<FpsCounter>("Right Color", logger_, 1);
fps_counter_depth_ = std::make_unique<FpsCounter>("Depth", logger_, 1);
fps_counter_left_ir_ = std::make_unique<FpsCounter>("Left Ir", logger_, 1);
fps_counter_right_ir_ = std::make_unique<FpsCounter>("Right Ir", logger_, 1);
@@ -770,12 +776,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
log_level = LogLevel::INFO;
}
fps_counter_color_->setLogLevel(log_level);
fps_counter_left_color_->setLogLevel(log_level);
fps_counter_right_color_->setLogLevel(log_level);
fps_counter_depth_->setLogLevel(log_level);
fps_counter_left_ir_->setLogLevel(log_level);
fps_counter_right_ir_->setLogLevel(log_level);
fps_delay_status_color_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_left_color_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_right_color_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_left_ir_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_right_ir_ = std::make_unique<FpsDelayStatus>(logger_);
}
template <class T>
@@ -6473,6 +6485,20 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
final_color_frame, final_depth_frame, frame_set_arrival_system_us,
frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected,
depth_publish_expected);
const auto record_side_stream = [&](const stream_index_pair &stream_index,
OBFrameType frame_type) {
auto frame = frame_set->getFrame(frame_type);
if (enable_stream_[stream_index] && frame) {
timestamp_csv_logger_->recordImageFrameArrival(stream_index.first, frame,
frame_set_arrival_system_us,
frame_set_arrival_steady_us, true);
}
};
record_side_stream(COLOR_LEFT, OB_FRAME_COLOR_LEFT);
record_side_stream(COLOR_RIGHT, OB_FRAME_COLOR_RIGHT);
record_side_stream(INFRA1, OB_FRAME_IR_LEFT);
record_side_stream(INFRA2, OB_FRAME_IR_RIGHT);
}
try {
@@ -6514,10 +6540,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
setColorAutoExposureROI();
left_color_frame = processColorFrameFilter(left_color_frame);
frame_set->pushFrame(left_color_frame);
fps_counter_left_color_->tick();
}
if (right_color_frame) {
right_color_frame = processColorFrameFilter(right_color_frame);
frame_set->pushFrame(right_color_frame);
fps_counter_right_color_->tick();
}
if (left_ir_frame) {
left_ir_frame = processLeftIrFrameFilter(left_ir_frame);
@@ -7117,13 +7145,19 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) {
if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) {
if (!has_raw_image_subscriber && log_image_timestamps) {
timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
getSteadyNowUs());
}
publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
if (!has_raw_image_subscriber && stream_index == COLOR) {
fps_delay_status_color_->tick(frame_timestamp);
if (!has_raw_image_subscriber) {
if (stream_index == COLOR) {
fps_delay_status_color_->tick(frame_timestamp);
} else if (stream_index == COLOR_LEFT) {
fps_delay_status_left_color_->tick(frame_timestamp);
} else {
fps_delay_status_right_color_->tick(frame_timestamp);
}
}
}
@@ -7148,10 +7182,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "left color frame is not decoded");
record_image_publish_skipped();
return;
}
if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "right color frame is not decoded");
record_image_publish_skipped();
return;
}
if (frame->getType() == OB_FRAME_COLOR) {
@@ -7211,8 +7247,16 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
if (stream_index == COLOR) {
fps_delay_status_color_->tick(frame_timestamp);
} else if (stream_index == COLOR_LEFT) {
fps_delay_status_left_color_->tick(frame_timestamp);
} else if (stream_index == COLOR_RIGHT) {
fps_delay_status_right_color_->tick(frame_timestamp);
} else if (stream_index == DEPTH) {
fps_delay_status_depth_->tick(frame_timestamp);
} else if (stream_index == INFRA1) {
fps_delay_status_left_ir_->tick(frame_timestamp);
} else if (stream_index == INFRA2) {
fps_delay_status_right_ir_->tick(frame_timestamp);
}
image_publishers_[stream_index]->publish(std::move(image_msg));
}
orbbec_camera/src/timestamp_csv_logger.cpp
+46 -1
View File
@@ -29,6 +29,18 @@ TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger)
depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH);
}
}
if (config.left_color_enabled) {
left_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_COLOR);
}
if (config.right_color_enabled) {
right_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_COLOR);
}
if (config.left_ir_enabled) {
left_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_IR);
}
if (config.right_ir_enabled) {
right_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_IR);
}
if (config.csv_file_path.empty()) {
return;
@@ -64,7 +76,12 @@ bool TimestampCsvLogger::enabled() const {
bool TimestampCsvLogger::imageEnabled() const {
return (synced_image_logger_ && synced_image_logger_->enabled()) ||
(color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled());
(color_logger_ && color_logger_->enabled()) ||
(left_color_logger_ && left_color_logger_->enabled()) ||
(right_color_logger_ && right_color_logger_->enabled()) ||
(depth_logger_ && depth_logger_->enabled()) ||
(left_ir_logger_ && left_ir_logger_->enabled()) ||
(right_ir_logger_ && right_ir_logger_->enabled());
}
bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const {
@@ -104,6 +121,18 @@ void TimestampCsvLogger::recordImageFrameSet(const std::shared_ptr<ob::Frame> &c
}
}
void TimestampCsvLogger::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) {
auto *timestamp_logger = imageLoggerForStream(stream_type);
if (timestamp_logger) {
timestamp_logger->recordStandaloneFrameArrival(stream_type, frame, arrival_system_us,
arrival_steady_us, image_publish_expected);
}
}
void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us,
@@ -164,7 +193,11 @@ void TimestampCsvLogger::shutdown() noexcept {
shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger");
shutdown_logger(color_logger_, "color timestamp CSV logger");
shutdown_logger(left_color_logger_, "left color timestamp CSV logger");
shutdown_logger(right_color_logger_, "right color timestamp CSV logger");
shutdown_logger(depth_logger_, "depth timestamp CSV logger");
shutdown_logger(left_ir_logger_, "left IR timestamp CSV logger");
shutdown_logger(right_ir_logger_, "right IR timestamp CSV logger");
shutdown_logger(synced_imu_logger_, "synced IMU timestamp CSV logger");
shutdown_logger(accel_logger_, "accel timestamp CSV logger");
shutdown_logger(gyro_logger_, "gyro timestamp CSV logger");
@@ -177,6 +210,18 @@ FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType s
if (stream_type == OB_STREAM_DEPTH) {
return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get();
}
if (stream_type == OB_STREAM_COLOR_LEFT) {
return left_color_logger_.get();
}
if (stream_type == OB_STREAM_COLOR_RIGHT) {
return right_color_logger_.get();
}
if (stream_type == OB_STREAM_IR_LEFT) {
return left_ir_logger_.get();
}
if (stream_type == OB_STREAM_IR_RIGHT) {
return right_ir_logger_.get();
}
return nullptr;
}