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

feat: enhance frame timestamp logging with expected interval and dropped frame tracking

Browse Source
This commit is contained in:
slz
2026-05-22 16:38:02 +08:00
parent 0f1816089c
commit 9b5fa0e14a
3 changed files with 100 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h
+5
View File
@@ -60,6 +60,7 @@ class FrameTimestampCsvLogger {
int64_t sdk_system_ts_us = 0;
int64_t arrival_system_us = 0;
int64_t arrival_steady_us = 0;
int64_t expected_interval_us = 0;
std::optional<int64_t> publish_system_us;
std::optional<int64_t> publish_steady_us;
@@ -85,6 +86,10 @@ class FrameTimestampCsvLogger {
struct PreviousStreamTimestamps {
std::optional<int64_t> device_ts_us;
std::optional<int64_t> publish_device_ts_us;
int64_t expected_interval_us = 0;
int64_t dropped_frames = 0;
int64_t publish_dropped_frames = 0;
std::optional<int64_t> sensor_ts_us;
std::optional<int64_t> global_ts_us;
std::optional<int64_t> sdk_system_ts_us;
orbbec_camera/src/frame_timestamp_csv_logger.cpp
+79 -29
View File
@@ -1,5 +1,6 @@
#include "orbbec_camera/frame_timestamp_csv_logger.h"
#include <algorithm>
#include <chrono>
#include <filesystem>
#include <iomanip>
@@ -13,6 +14,21 @@ constexpr size_t kCompletedQueueSoftLimit = 1000;
constexpr size_t kFlushBatchSize = 100;
constexpr auto kFlushInterval = std::chrono::seconds(1);
int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
if (!frame || !frame->is<ob::VideoFrame>()) {
return 0;
}
auto stream_profile = frame->getStreamProfile();
if (!stream_profile || !stream_profile->is<ob::VideoStreamProfile>()) {
return 0;
}
const auto fps = stream_profile->as<ob::VideoStreamProfile>()->fps();
if (fps == 0) {
return 0;
}
return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
}
} // namespace
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path,
@@ -22,6 +38,19 @@ FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool enabled, const std::string
return;
}
RCLCPP_INFO_STREAM(
logger_,
"Frame drop log enabled. Format: <SDK|PUB> drop <color|depth>: idx=<frame_index> "
"ts=<device_timestamp_us> gap=<actual_interval_ms> ideal=<expected_interval_ms> "
"total=<total_dropped_frames>. SDK means frame arrival from SDK; PUB means before ROS "
"image publish.");
if (csv_file_path_.empty()) {
RCLCPP_INFO_STREAM(logger_,
"Frame timestamp CSV file is empty; only frame lost logs are enabled.");
return;
}
try {
auto path = std::filesystem::path(csv_file_path_);
if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) {
@@ -284,6 +313,27 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
state.has_frame = true;
state.publish_expected = publish_expected;
state.frame_index = frame->index();
state.device_ts_us = static_cast<int64_t>(frame->timeStampUs());
if (previous.expected_interval_us <= 0) {
previous.expected_interval_us = getExpectedIntervalUs(frame);
}
state.expected_interval_us = previous.expected_interval_us;
if (previous.device_ts_us.has_value() && state.expected_interval_us > 0) {
const auto device_ts_delta_us = state.device_ts_us - previous.device_ts_us.value();
if (device_ts_delta_us > state.expected_interval_us * 3 / 2) {
const auto lost_frames =
std::max<int64_t>(1, device_ts_delta_us / state.expected_interval_us - 1);
previous.dropped_frames += lost_frames;
RCLCPP_WARN_STREAM(
logger_, "SDK drop " << (stream == TrackedStream::COLOR ? "color" : "depth") << ": idx="
<< state.frame_index << " ts=" << state.device_ts_us << "us"
<< " gap=" << std::fixed << std::setprecision(1)
<< (static_cast<double>(device_ts_delta_us) / 1000.0) << "ms"
<< " ideal="
<< (static_cast<double>(state.expected_interval_us) / 1000.0) << "ms"
<< " total=" << previous.dropped_frames);
}
}
if (frame->hasMetadata(OB_FRAME_METADATA_TYPE_FRAME_NUMBER)) {
state.metadata_frame_number =
static_cast<int64_t>(frame->getMetadataValue(OB_FRAME_METADATA_TYPE_FRAME_NUMBER));
@@ -296,7 +346,6 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
} else {
state.sensor_ts_us.reset();
}
state.device_ts_us = static_cast<int64_t>(frame->timeStampUs());
state.global_ts_us = static_cast<int64_t>(frame->globalTimeStampUs());
state.sdk_system_ts_us = static_cast<int64_t>(frame->systemTimeStampUs());
state.arrival_system_us = arrival_system_us;
@@ -310,7 +359,7 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
}
state.global_ts_delta_us = updateDelta(previous.global_ts_us, state.global_ts_us);
state.sdk_system_ts_delta_us = updateDelta(previous.sdk_system_ts_us, state.sdk_system_ts_us);
state.arrival_system_delta_us = updateDelta(previous.arrival_system_us, state.arrival_system_us);
previous.arrival_system_us = state.arrival_system_us;
state.arrival_steady_delta_us = updateDelta(previous.arrival_steady_us, state.arrival_steady_us);
state.sdk_delay_from_global_us = state.arrival_system_us - state.global_ts_us;
state.sdk_delay_from_system_us = state.arrival_system_us - state.sdk_system_ts_us;
@@ -321,13 +370,29 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
int64_t publish_steady_us) {
auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_;
if (previous.publish_device_ts_us.has_value()) {
const auto device_ts_delta_us = state.device_ts_us - previous.publish_device_ts_us.value();
if (state.expected_interval_us > 0 && device_ts_delta_us > state.expected_interval_us * 3 / 2) {
const auto lost_frames =
std::max<int64_t>(1, device_ts_delta_us / state.expected_interval_us - 1);
previous.publish_dropped_frames += lost_frames;
RCLCPP_WARN_STREAM(
logger_, "PUB drop " << (stream == TrackedStream::COLOR ? "color" : "depth") << ": idx="
<< state.frame_index << " ts=" << state.device_ts_us << "us"
<< " gap=" << std::fixed << std::setprecision(1)
<< (static_cast<double>(device_ts_delta_us) / 1000.0) << "ms"
<< " ideal="
<< (static_cast<double>(state.expected_interval_us) / 1000.0) << "ms"
<< " total=" << previous.publish_dropped_frames);
}
}
previous.publish_device_ts_us = state.device_ts_us;
state.publish_system_us = publish_system_us;
state.publish_steady_us = publish_steady_us;
state.publish_system_delta_us =
updateDelta(previous.publish_system_us, state.publish_system_us.value());
previous.publish_system_us = state.publish_system_us.value();
state.publish_steady_delta_us =
updateDelta(previous.publish_steady_us, state.publish_steady_us.value());
state.arrival_to_publish_system_us = state.publish_system_us.value() - state.arrival_system_us;
state.arrival_to_publish_steady_us = state.publish_steady_us.value() - state.arrival_steady_us;
}
@@ -350,6 +415,9 @@ bool FrameTimestampCsvLogger::isRowReady(const PendingRow &row) const {
}
void FrameTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) {
if (csv_file_path_.empty()) {
return;
}
std::lock_guard<std::mutex> queue_lock(completed_rows_mutex_);
completed_rows_.push_back(row);
if (completed_rows_.size() > kCompletedQueueSoftLimit) {
@@ -393,7 +461,7 @@ std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
}
std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &state) const {
std::vector<std::string> fields(22, "");
std::vector<std::string> fields(15, "");
if (state.has_frame) {
fields[0] = std::to_string(state.frame_index);
fields[1] = formatOptionalIntColumn(state.metadata_frame_number);
@@ -407,22 +475,11 @@ std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &s
fields[7] = formatOptionalIntColumn(state.global_ts_delta_us);
fields[8] = formatSecondsColumn(state.sdk_system_ts_us);
fields[9] = formatOptionalIntColumn(state.sdk_system_ts_delta_us);
fields[10] = formatSecondsColumn(state.arrival_system_us);
fields[11] = formatOptionalIntColumn(state.arrival_system_delta_us);
fields[12] = formatSecondsColumn(state.arrival_steady_us);
fields[13] = formatOptionalIntColumn(state.arrival_steady_delta_us);
if (state.publish_system_us.has_value()) {
fields[14] = formatSecondsColumn(state.publish_system_us.value());
}
fields[15] = formatOptionalIntColumn(state.publish_system_delta_us);
if (state.publish_steady_us.has_value()) {
fields[16] = formatSecondsColumn(state.publish_steady_us.value());
}
fields[17] = formatOptionalIntColumn(state.publish_steady_delta_us);
fields[18] = formatOptionalIntColumn(state.arrival_to_publish_system_us);
fields[19] = formatOptionalIntColumn(state.arrival_to_publish_steady_us);
fields[20] = formatOptionalIntColumn(state.sdk_delay_from_global_us);
fields[21] = formatOptionalIntColumn(state.sdk_delay_from_system_us);
fields[10] = formatOptionalIntColumn(state.arrival_steady_delta_us);
fields[11] = formatOptionalIntColumn(state.publish_steady_delta_us);
fields[12] = formatOptionalIntColumn(state.arrival_to_publish_steady_us);
fields[13] = formatOptionalIntColumn(state.sdk_delay_from_global_us);
fields[14] = formatOptionalIntColumn(state.sdk_delay_from_system_us);
}
std::ostringstream ss;
@@ -461,15 +518,8 @@ std::string FrameTimestampCsvLogger::csvHeader() {
ss << prefix << "_global_ts_delta_us,";
ss << prefix << "_system_ts_sec,";
ss << prefix << "_system_ts_delta_us,";
ss << prefix << "_arrival_system_sec,";
ss << prefix << "_arrival_system_delta_us,";
ss << prefix << "_arrival_steady_sec,";
ss << prefix << "_arrival_steady_delta_us,";
ss << prefix << "_publish_system_sec,";
ss << prefix << "_publish_system_delta_us,";
ss << prefix << "_publish_steady_sec,";
ss << prefix << "_publish_steady_delta_us,";
ss << prefix << "_arrival_to_publish_system_us,";
ss << prefix << "_arrival_to_publish_steady_us,";
ss << prefix << "_sdk_delay_from_global_us,";
ss << prefix << "_sdk_delay_from_system_us";
orbbec_camera/src/ob_camera_node.cpp
+16 -19
View File
@@ -75,11 +75,6 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
setupDefaultImageFormat();
setupTopics();
if (enable_frame_timestamp_csv_) {
if (frame_timestamp_csv_file_.empty()) {
frame_timestamp_csv_file_ =
(std::filesystem::current_path() / (camera_name_ + "_frame_timestamp_stats.csv"))
.string();
}
frame_timestamp_csv_logger_ =
std::make_unique<FrameTimestampCsvLogger>(true, frame_timestamp_csv_file_, logger_);
if (!frame_timestamp_csv_logger_->enabled()) {
@@ -1854,8 +1849,22 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
if (frame_set == nullptr) {
return;
}
const auto frame_set_arrival_system_us = getSystemNowUs();
const auto frame_set_arrival_steady_us = getSteadyNowUs();
if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) {
const auto frame_set_arrival_system_us = getSystemNowUs();
const auto frame_set_arrival_steady_us = getSteadyNowUs();
auto final_color_frame = frame_set->getFrame(OB_FRAME_COLOR);
auto final_depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
const bool track_color = enable_stream_[COLOR] && static_cast<bool>(final_color_frame);
const bool track_depth = enable_stream_[DEPTH] && static_cast<bool>(final_depth_frame);
const bool color_publish_expected = track_color;
const bool depth_publish_expected = track_depth;
frame_timestamp_csv_logger_->recordFrameSet(
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);
}
try {
if (!tf_published_) {
publishStaticTransforms();
@@ -1884,18 +1893,6 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
"null or color frame is null");
}
}
auto final_color_frame = frame_set->getFrame(OB_FRAME_COLOR);
auto final_depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) {
const bool track_color = enable_stream_[COLOR] && static_cast<bool>(final_color_frame);
const bool track_depth = enable_stream_[DEPTH] && static_cast<bool>(final_depth_frame);
const bool color_publish_expected = track_color;
const bool depth_publish_expected = track_depth;
frame_timestamp_csv_logger_->recordFrameSet(
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);
}
if (enable_stream_[COLOR] && color_frame) {
std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
// if (color_frame_queue_.size() > 2) {