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

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<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

@@ -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 =
+  previous.publish_system_us = state.publish_system_us.value();
-      updateDelta(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[10] = formatOptionalIntColumn(state.arrival_steady_delta_us);
-    fields[11] = formatOptionalIntColumn(state.arrival_system_delta_us);
+    fields[11] = formatOptionalIntColumn(state.publish_steady_delta_us);
-    fields[12] = formatSecondsColumn(state.arrival_steady_us);
+    fields[12] = formatOptionalIntColumn(state.arrival_to_publish_steady_us);
-    fields[13] = formatOptionalIntColumn(state.arrival_steady_delta_us);
+    fields[13] = formatOptionalIntColumn(state.sdk_delay_from_global_us);
-    if (state.publish_system_us.has_value()) {
+    fields[14] = formatOptionalIntColumn(state.sdk_delay_from_system_us);
-      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);
   }
 
   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

@@ -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();
+  if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) {
-  const auto frame_set_arrival_steady_us = getSteadyNowUs();
+    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) {