From 9b5fa0e14a0f2822fad4c69ac464b7acb8e81aab Mon Sep 17 00:00:00 2001 From: slz Date: Fri, 22 May 2026 16:38:02 +0800 Subject: [PATCH] feat: enhance frame timestamp logging with expected interval and dropped frame tracking --- .../frame_timestamp_csv_logger.h | 5 + .../src/frame_timestamp_csv_logger.cpp | 108 +++++++++++++----- orbbec_camera/src/ob_camera_node.cpp | 35 +++--- 3 files changed, 100 insertions(+), 48 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h index aed2e035..5ede4045 100644 --- a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h +++ b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h @@ -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 publish_system_us; std::optional publish_steady_us; @@ -85,6 +86,10 @@ class FrameTimestampCsvLogger { struct PreviousStreamTimestamps { std::optional device_ts_us; + std::optional publish_device_ts_us; + int64_t expected_interval_us = 0; + int64_t dropped_frames = 0; + int64_t publish_dropped_frames = 0; std::optional sensor_ts_us; std::optional global_ts_us; std::optional sdk_system_ts_us; diff --git a/orbbec_camera/src/frame_timestamp_csv_logger.cpp b/orbbec_camera/src/frame_timestamp_csv_logger.cpp index a3ae7e83..03ca0132 100644 --- a/orbbec_camera/src/frame_timestamp_csv_logger.cpp +++ b/orbbec_camera/src/frame_timestamp_csv_logger.cpp @@ -1,5 +1,6 @@ #include "orbbec_camera/frame_timestamp_csv_logger.h" +#include #include #include #include @@ -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 &frame) { + if (!frame || !frame->is()) { + return 0; + } + auto stream_profile = frame->getStreamProfile(); + if (!stream_profile || !stream_profile->is()) { + return 0; + } + const auto fps = stream_profile->as()->fps(); + if (fps == 0) { + return 0; + } + return static_cast(1000000.0 / static_cast(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: drop : idx= " + "ts= gap= ideal= " + "total=. 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(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(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(device_ts_delta_us) / 1000.0) << "ms" + << " ideal=" + << (static_cast(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(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(frame->timeStampUs()); state.global_ts_us = static_cast(frame->globalTimeStampUs()); state.sdk_system_ts_us = static_cast(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(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(device_ts_delta_us) / 1000.0) << "ms" + << " ideal=" + << (static_cast(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 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 fields(22, ""); + std::vector 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"; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 1e7e82ba..7ec59bea 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -75,11 +75,6 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr 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(true, frame_timestamp_csv_file_, logger_); if (!frame_timestamp_csv_logger_->enabled()) { @@ -1854,8 +1849,22 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr 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(final_color_frame); + const bool track_depth = enable_stream_[DEPTH] && static_cast(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 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(final_color_frame); - const bool track_depth = enable_stream_[DEPTH] && static_cast(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 lock(color_frame_queue_lock_); // if (color_frame_queue_.size() > 2) {