feat: add frame timestamp CSV logging functionality

orbbec_camera/CMakeLists.txt

@@ -135,6 +135,7 @@ set(SOURCE_FILES
   src/d2c_viewer.cpp
   src/dynamic_params.cpp
   src/image_publisher.cpp
+  src/frame_timestamp_csv_logger.cpp
   src/ob_camera_node_driver.cpp
   src/ob_camera_node.cpp
   src/ros_param_backend.cpp

orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h

@@ -0,0 +1,155 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <atomic>
+#include <condition_variable>
+#include <cstdint>
+#include <deque>
+#include <fstream>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <thread>
+#include <unordered_map>
+#include <vector>
+
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+
+class FrameTimestampCsvLogger {
+ public:
+  FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path, rclcpp::Logger logger);
+
+  ~FrameTimestampCsvLogger() noexcept;
+
+  FrameTimestampCsvLogger(const FrameTimestampCsvLogger &) = delete;
+  FrameTimestampCsvLogger &operator=(const FrameTimestampCsvLogger &) = delete;
+
+  void recordFrameSet(const std::shared_ptr<ob::Frame> &color_frame,
+                      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 recordStandaloneFrameArrival(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 recordPreImagePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                             int64_t publish_system_us, int64_t publish_steady_us);
+
+  void shutdown();
+
+  bool enabled() const { return enabled_; }
+
+ private:
+  enum class TrackedStream { COLOR, DEPTH };
+
+  struct StreamState {
+    bool has_frame = false;
+    bool publish_expected = false;
+    bool final = false;
+    uint64_t frame_index = 0;
+
+    int64_t device_ts_us = 0;
+    int64_t global_ts_us = 0;
+    int64_t sdk_system_ts_us = 0;
+    int64_t arrival_system_us = 0;
+    int64_t arrival_steady_us = 0;
+    std::optional<int64_t> publish_system_us;
+    std::optional<int64_t> publish_steady_us;
+
+    std::optional<int64_t> device_ts_delta_us;
+    std::optional<int64_t> global_ts_delta_us;
+    std::optional<int64_t> sdk_system_ts_delta_us;
+    std::optional<int64_t> arrival_system_delta_us;
+    std::optional<int64_t> arrival_steady_delta_us;
+    std::optional<int64_t> publish_system_delta_us;
+    std::optional<int64_t> publish_steady_delta_us;
+    std::optional<int64_t> arrival_to_publish_system_us;
+    std::optional<int64_t> arrival_to_publish_steady_us;
+    std::optional<int64_t> sdk_delay_from_global_us;
+    std::optional<int64_t> sdk_delay_from_system_us;
+  };
+
+  struct PendingRow {
+    uint64_t row_id = 0;
+    StreamState color;
+    StreamState depth;
+  };
+
+  struct PreviousStreamTimestamps {
+    std::optional<int64_t> device_ts_us;
+    std::optional<int64_t> global_ts_us;
+    std::optional<int64_t> sdk_system_ts_us;
+    std::optional<int64_t> arrival_system_us;
+    std::optional<int64_t> arrival_steady_us;
+    std::optional<int64_t> publish_system_us;
+    std::optional<int64_t> publish_steady_us;
+  };
+
+  TrackedStream toTrackedStream(OBStreamType stream_type) const;
+  bool isTrackedStream(OBStreamType stream_type) const;
+
+  void recordFrameSetInternal(const std::shared_ptr<ob::Frame> &color_frame,
+                              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 recordStandaloneFrameArrivalInternal(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 recordPreImagePublishInternal(OBStreamType stream_type,
+                                     const std::shared_ptr<ob::Frame> &frame,
+                                     int64_t publish_system_us, int64_t publish_steady_us);
+
+  void populateArrivalData(StreamState &state, TrackedStream stream,
+                           const std::shared_ptr<ob::Frame> &frame, int64_t arrival_system_us,
+                           int64_t arrival_steady_us, bool publish_expected);
+  void populatePublishData(StreamState &state, TrackedStream stream, int64_t publish_system_us,
+                           int64_t publish_steady_us);
+
+  std::optional<int64_t> updateDelta(std::optional<int64_t> &previous, int64_t current);
+
+  void finalizeStreamWithoutPublish(StreamState &state);
+  bool isRowReady(const PendingRow &row) const;
+  void enqueueCompletedRow(const PendingRow &row);
+  void flushPendingRowsLocked(std::vector<PendingRow> &rows);
+  void eraseFrameIndexMappingLocked(const PendingRow &row);
+
+  std::string serializeRow(const PendingRow &row) const;
+  std::string serializeStreamColumns(const StreamState &state) const;
+  static std::string formatSecondsColumn(int64_t time_us);
+  static std::string formatOptionalIntColumn(const std::optional<int64_t> &value);
+  static std::string csvHeader();
+
+  void writerThreadMain();
+  void openCsvIfNeeded();
+
+  rclcpp::Logger logger_;
+  bool enabled_ = false;
+  std::atomic_bool shutdown_requested_{false};
+  bool writer_failed_ = false;
+  bool queue_warning_active_ = false;
+  std::string csv_file_path_;
+  std::ofstream csv_stream_;
+  std::thread writer_thread_;
+
+  uint64_t next_row_id_ = 1;
+  std::unordered_map<uint64_t, PendingRow> pending_rows_;
+  std::unordered_map<uint64_t, uint64_t> color_frame_index_to_row_id_;
+  std::unordered_map<uint64_t, uint64_t> depth_frame_index_to_row_id_;
+  PreviousStreamTimestamps color_previous_;
+  PreviousStreamTimestamps depth_previous_;
+
+  std::deque<PendingRow> completed_rows_;
+
+  mutable std::mutex state_mutex_;
+  std::mutex completed_rows_mutex_;
+  std::condition_variable completed_rows_cv_;
+};
+
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -61,6 +61,7 @@
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
 #include "orbbec_camera/image_publisher.h"
+#include "orbbec_camera/frame_timestamp_csv_logger.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/string.hpp>
 
@@ -584,6 +585,9 @@ class OBCameraNode {
   int min_depth_limit_ = 0;
   int max_depth_limit_ = 0;
   std::string time_domain_ = "device";  // device, system, global
+  bool enable_frame_timestamp_csv_ = false;
+  std::string frame_timestamp_csv_file_;
+  std::unique_ptr<FrameTimestampCsvLogger> frame_timestamp_csv_logger_;
   // soft ware trigger
   rclcpp::TimerBase::SharedPtr software_trigger_timer_;
   std::chrono::milliseconds software_trigger_period_{33};

orbbec_camera/launch/gemini_330_series.launch.py

@@ -207,6 +207,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_3d_reconstruction_mode', default_value='false'),
         DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
         DeclareLaunchArgument('time_domain', default_value='device'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),

orbbec_camera/src/frame_timestamp_csv_logger.cpp

@@ -0,0 +1,510 @@
+#include "orbbec_camera/frame_timestamp_csv_logger.h"
+
+#include <chrono>
+#include <filesystem>
+#include <iomanip>
+#include <sstream>
+#include <utility>
+
+namespace orbbec_camera {
+namespace {
+
+constexpr size_t kCompletedQueueSoftLimit = 1000;
+constexpr size_t kFlushBatchSize = 100;
+constexpr auto kFlushInterval = std::chrono::seconds(1);
+
+}  // namespace
+
+FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path,
+                                                 rclcpp::Logger logger)
+    : logger_(std::move(logger)), enabled_(enabled), csv_file_path_(csv_file_path) {
+  if (!enabled_) {
+    return;
+  }
+
+  try {
+    auto path = std::filesystem::path(csv_file_path_);
+    if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) {
+      std::filesystem::create_directories(path.parent_path());
+    }
+  } catch (const std::exception &e) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to prepare frame timestamp CSV path " << csv_file_path_
+                                                                               << ": " << e.what());
+    enabled_ = false;
+    writer_failed_ = true;
+    return;
+  }
+
+  openCsvIfNeeded();
+  if (!enabled_) {
+    return;
+  }
+
+  writer_thread_ = std::thread([this]() { writerThreadMain(); });
+}
+
+FrameTimestampCsvLogger::~FrameTimestampCsvLogger() noexcept { shutdown(); }
+
+void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &color_frame,
+                                             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) {
+  if (!enabled_ || writer_failed_) {
+    return;
+  }
+  recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us,
+                         track_color, track_depth, color_image_publish_expected,
+                         depth_image_publish_expected);
+}
+
+void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_type,
+                                                           const std::shared_ptr<ob::Frame> &frame,
+                                                           int64_t arrival_system_us,
+                                                           int64_t arrival_steady_us,
+                                                           bool image_publish_expected) {
+  if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
+    return;
+  }
+  recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
+                                       image_publish_expected);
+}
+
+void FrameTimestampCsvLogger::recordPreImagePublish(OBStreamType stream_type,
+                                                    const std::shared_ptr<ob::Frame> &frame,
+                                                    int64_t publish_system_us,
+                                                    int64_t publish_steady_us) {
+  if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
+    return;
+  }
+  recordPreImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
+}
+
+void FrameTimestampCsvLogger::shutdown() {
+  if (!enabled_) {
+    return;
+  }
+
+  {
+    std::lock_guard<std::mutex> state_lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+    shutdown_requested_ = true;
+
+    std::vector<PendingRow> rows_to_flush;
+    flushPendingRowsLocked(rows_to_flush);
+    for (const auto &row : rows_to_flush) {
+      enqueueCompletedRow(row);
+    }
+  }
+
+  completed_rows_cv_.notify_all();
+  if (writer_thread_.joinable()) {
+    writer_thread_.join();
+  }
+
+  if (csv_stream_.is_open()) {
+    csv_stream_.flush();
+    csv_stream_.close();
+  }
+}
+
+FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream(
+    OBStreamType stream_type) const {
+  if (stream_type == OB_STREAM_COLOR) {
+    return TrackedStream::COLOR;
+  }
+  return TrackedStream::DEPTH;
+}
+
+bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const {
+  return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH;
+}
+
+void FrameTimestampCsvLogger::recordFrameSetInternal(
+    const std::shared_ptr<ob::Frame> &color_frame, 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) {
+  if (!track_color && !track_depth) {
+    return;
+  }
+
+  std::vector<PendingRow> ready_rows;
+  {
+    std::lock_guard<std::mutex> lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+
+    PendingRow row;
+    row.row_id = next_row_id_++;
+
+    if (track_color && color_frame) {
+      populateArrivalData(row.color, TrackedStream::COLOR, color_frame, arrival_system_us,
+                          arrival_steady_us, color_image_publish_expected);
+      color_frame_index_to_row_id_[row.color.frame_index] = row.row_id;
+      if (!color_image_publish_expected) {
+        finalizeStreamWithoutPublish(row.color);
+      }
+    } else {
+      row.color.final = true;
+    }
+
+    if (track_depth && depth_frame) {
+      populateArrivalData(row.depth, TrackedStream::DEPTH, depth_frame, arrival_system_us,
+                          arrival_steady_us, depth_image_publish_expected);
+      depth_frame_index_to_row_id_[row.depth.frame_index] = row.row_id;
+      if (!depth_image_publish_expected) {
+        finalizeStreamWithoutPublish(row.depth);
+      }
+    } else {
+      row.depth.final = true;
+    }
+
+    pending_rows_.emplace(row.row_id, row);
+    if (isRowReady(row)) {
+      auto it = pending_rows_.find(row.row_id);
+      if (it != pending_rows_.end()) {
+        ready_rows.push_back(it->second);
+        eraseFrameIndexMappingLocked(it->second);
+        pending_rows_.erase(it);
+      }
+    }
+  }
+
+  for (const auto &ready_row : ready_rows) {
+    enqueueCompletedRow(ready_row);
+  }
+}
+
+void FrameTimestampCsvLogger::recordStandaloneFrameArrivalInternal(
+    OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame, int64_t arrival_system_us,
+    int64_t arrival_steady_us, bool image_publish_expected) {
+  std::optional<PendingRow> ready_row;
+  {
+    std::lock_guard<std::mutex> lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+
+    PendingRow row;
+    row.row_id = next_row_id_++;
+
+    const auto tracked_stream = toTrackedStream(stream_type);
+    auto &state = tracked_stream == TrackedStream::COLOR ? row.color : row.depth;
+    auto &other_state = tracked_stream == TrackedStream::COLOR ? row.depth : row.color;
+
+    populateArrivalData(state, tracked_stream, frame, arrival_system_us, arrival_steady_us,
+                        image_publish_expected);
+    other_state.final = true;
+
+    if (tracked_stream == TrackedStream::COLOR) {
+      color_frame_index_to_row_id_[state.frame_index] = row.row_id;
+    } else {
+      depth_frame_index_to_row_id_[state.frame_index] = row.row_id;
+    }
+
+    if (!image_publish_expected) {
+      finalizeStreamWithoutPublish(state);
+    }
+
+    pending_rows_.emplace(row.row_id, row);
+    if (isRowReady(row)) {
+      auto it = pending_rows_.find(row.row_id);
+      if (it != pending_rows_.end()) {
+        ready_row = it->second;
+        eraseFrameIndexMappingLocked(*ready_row);
+        pending_rows_.erase(it);
+      }
+    }
+  }
+
+  if (ready_row.has_value()) {
+    enqueueCompletedRow(*ready_row);
+  }
+}
+
+void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_type,
+                                                            const std::shared_ptr<ob::Frame> &frame,
+                                                            int64_t publish_system_us,
+                                                            int64_t publish_steady_us) {
+  std::optional<PendingRow> ready_row;
+  const auto frame_index = frame->getIndex();
+
+  {
+    std::lock_guard<std::mutex> lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+
+    const auto tracked_stream = toTrackedStream(stream_type);
+    auto &row_map = tracked_stream == TrackedStream::COLOR ? color_frame_index_to_row_id_
+                                                           : depth_frame_index_to_row_id_;
+    auto row_id_it = row_map.find(frame_index);
+    if (row_id_it == row_map.end()) {
+      RCLCPP_WARN_STREAM(logger_,
+                         "Frame timestamp CSV logger missed row mapping for stream "
+                             << (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
+                             << " frame index " << frame_index);
+      return;
+    }
+    const auto row_id = row_id_it->second;
+
+    auto pending_it = pending_rows_.find(row_id);
+    if (pending_it == pending_rows_.end()) {
+      return;
+    }
+
+    auto &state = tracked_stream == TrackedStream::COLOR ? pending_it->second.color
+                                                         : pending_it->second.depth;
+    populatePublishData(state, tracked_stream, publish_system_us, publish_steady_us);
+    state.final = true;
+
+    if (isRowReady(pending_it->second)) {
+      ready_row = pending_it->second;
+      eraseFrameIndexMappingLocked(*ready_row);
+      pending_rows_.erase(pending_it);
+    }
+  }
+
+  if (ready_row.has_value()) {
+    enqueueCompletedRow(*ready_row);
+  }
+}
+
+void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStream stream,
+                                                  const std::shared_ptr<ob::Frame> &frame,
+                                                  int64_t arrival_system_us,
+                                                  int64_t arrival_steady_us,
+                                                  bool publish_expected) {
+  auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_;
+
+  state.has_frame = true;
+  state.publish_expected = publish_expected;
+  state.frame_index = frame->getIndex();
+  state.device_ts_us = static_cast<int64_t>(frame->getTimeStampUs());
+  state.global_ts_us = static_cast<int64_t>(frame->getGlobalTimeStampUs());
+  state.sdk_system_ts_us = static_cast<int64_t>(frame->getSystemTimeStampUs());
+  state.arrival_system_us = arrival_system_us;
+  state.arrival_steady_us = arrival_steady_us;
+  state.device_ts_delta_us = updateDelta(previous.device_ts_us, state.device_ts_us);
+  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);
+  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;
+}
+
+void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStream stream,
+                                                  int64_t publish_system_us,
+                                                  int64_t publish_steady_us) {
+  auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_;
+
+  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());
+  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;
+}
+
+std::optional<int64_t> FrameTimestampCsvLogger::updateDelta(std::optional<int64_t> &previous,
+                                                            int64_t current) {
+  std::optional<int64_t> delta;
+  if (previous.has_value()) {
+    delta = current - previous.value();
+  }
+  previous = current;
+  return delta;
+}
+
+void FrameTimestampCsvLogger::finalizeStreamWithoutPublish(StreamState &state) {
+  state.final = true;
+}
+
+bool FrameTimestampCsvLogger::isRowReady(const PendingRow &row) const {
+  return row.color.final && row.depth.final;
+}
+
+void FrameTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) {
+  std::lock_guard<std::mutex> queue_lock(completed_rows_mutex_);
+  completed_rows_.push_back(row);
+  if (completed_rows_.size() > kCompletedQueueSoftLimit) {
+    if (!queue_warning_active_) {
+      RCLCPP_WARN_STREAM(logger_, "Frame timestamp CSV queue size exceeded "
+                                      << kCompletedQueueSoftLimit << " rows");
+      queue_warning_active_ = true;
+    }
+  } else {
+    queue_warning_active_ = false;
+  }
+  completed_rows_cv_.notify_one();
+}
+
+void FrameTimestampCsvLogger::flushPendingRowsLocked(std::vector<PendingRow> &rows) {
+  rows.reserve(rows.size() + pending_rows_.size());
+  for (auto &item : pending_rows_) {
+    auto row = item.second;
+    row.color.final = true;
+    row.depth.final = true;
+    rows.push_back(std::move(row));
+  }
+  pending_rows_.clear();
+  color_frame_index_to_row_id_.clear();
+  depth_frame_index_to_row_id_.clear();
+}
+
+void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row) {
+  if (row.color.has_frame) {
+    color_frame_index_to_row_id_.erase(row.color.frame_index);
+  }
+  if (row.depth.has_frame) {
+    depth_frame_index_to_row_id_.erase(row.depth.frame_index);
+  }
+}
+
+std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
+  std::ostringstream ss;
+  ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
+  return ss.str();
+}
+
+std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &state) const {
+  std::vector<std::string> fields(19, "");
+  if (state.has_frame) {
+    fields[0] = std::to_string(state.frame_index);
+    fields[1] = formatSecondsColumn(state.device_ts_us);
+    fields[2] = formatOptionalIntColumn(state.device_ts_delta_us);
+    fields[3] = formatSecondsColumn(state.global_ts_us);
+    fields[4] = formatOptionalIntColumn(state.global_ts_delta_us);
+    fields[5] = formatSecondsColumn(state.sdk_system_ts_us);
+    fields[6] = formatOptionalIntColumn(state.sdk_system_ts_delta_us);
+    fields[7] = formatSecondsColumn(state.arrival_system_us);
+    fields[8] = formatOptionalIntColumn(state.arrival_system_delta_us);
+    fields[9] = formatSecondsColumn(state.arrival_steady_us);
+    fields[10] = formatOptionalIntColumn(state.arrival_steady_delta_us);
+    if (state.publish_system_us.has_value()) {
+      fields[11] = formatSecondsColumn(state.publish_system_us.value());
+    }
+    fields[12] = formatOptionalIntColumn(state.publish_system_delta_us);
+    if (state.publish_steady_us.has_value()) {
+      fields[13] = formatSecondsColumn(state.publish_steady_us.value());
+    }
+    fields[14] = formatOptionalIntColumn(state.publish_steady_delta_us);
+    fields[15] = formatOptionalIntColumn(state.arrival_to_publish_system_us);
+    fields[16] = formatOptionalIntColumn(state.arrival_to_publish_steady_us);
+    fields[17] = formatOptionalIntColumn(state.sdk_delay_from_global_us);
+    fields[18] = formatOptionalIntColumn(state.sdk_delay_from_system_us);
+  }
+
+  std::ostringstream ss;
+  for (size_t i = 0; i < fields.size(); ++i) {
+    if (i != 0) {
+      ss << ",";
+    }
+    ss << fields[i];
+  }
+  return ss.str();
+}
+
+std::string FrameTimestampCsvLogger::formatSecondsColumn(int64_t time_us) {
+  std::ostringstream ss;
+  ss << std::fixed << std::setprecision(6) << (static_cast<long double>(time_us) / 1000000.0L);
+  return ss.str();
+}
+
+std::string FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional<int64_t> &value) {
+  if (!value.has_value()) {
+    return "";
+  }
+  return std::to_string(*value);
+}
+
+std::string FrameTimestampCsvLogger::csvHeader() {
+  std::ostringstream ss;
+  for (const auto *prefix : {"color", "depth"}) {
+    ss << prefix << "_frame_index,";
+    ss << prefix << "_device_ts_sec,";
+    ss << prefix << "_device_ts_delta_us,";
+    ss << prefix << "_global_ts_sec,";
+    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";
+    if (std::string(prefix) == "color") {
+      ss << ",";
+    }
+  }
+  return ss.str();
+}
+
+void FrameTimestampCsvLogger::writerThreadMain() {
+  if (!enabled_ || writer_failed_) {
+    return;
+  }
+
+  size_t rows_since_flush = 0;
+  auto last_flush = std::chrono::steady_clock::now();
+
+  while (true) {
+    std::deque<PendingRow> rows_to_write;
+    {
+      std::unique_lock<std::mutex> lock(completed_rows_mutex_);
+      completed_rows_cv_.wait_for(lock, kFlushInterval, [this]() {
+        return shutdown_requested_ || !completed_rows_.empty();
+      });
+      rows_to_write.swap(completed_rows_);
+    }
+
+    for (const auto &row : rows_to_write) {
+      if (csv_stream_.is_open()) {
+        csv_stream_ << serializeRow(row) << "\n";
+        rows_since_flush++;
+      }
+    }
+
+    const auto now = std::chrono::steady_clock::now();
+    if (csv_stream_.is_open() && (rows_since_flush >= kFlushBatchSize ||
+                                  now - last_flush >= kFlushInterval || shutdown_requested_)) {
+      csv_stream_.flush();
+      rows_since_flush = 0;
+      last_flush = now;
+    }
+
+    std::lock_guard<std::mutex> lock(completed_rows_mutex_);
+    if (shutdown_requested_ && completed_rows_.empty()) {
+      break;
+    }
+  }
+}
+
+void FrameTimestampCsvLogger::openCsvIfNeeded() {
+  csv_stream_.open(csv_file_path_, std::ios::out | std::ios::trunc);
+  if (!csv_stream_.is_open()) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to open frame timestamp CSV file: " << csv_file_path_);
+    enabled_ = false;
+    writer_failed_ = true;
+    return;
+  }
+  csv_stream_ << csvHeader() << "\n";
+  csv_stream_.flush();
+  RCLCPP_INFO_STREAM(logger_, "Frame timestamp CSV logger enabled: " << csv_file_path_);
+}
+
+}  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -34,6 +34,22 @@
 namespace orbbec_camera {
 using namespace std::chrono_literals;
 
+namespace {
+
+int64_t getSystemNowUs() {
+  return std::chrono::duration_cast<std::chrono::microseconds>(
+             std::chrono::system_clock::now().time_since_epoch())
+      .count();
+}
+
+int64_t getSteadyNowUs() {
+  return std::chrono::duration_cast<std::chrono::microseconds>(
+             std::chrono::steady_clock::now().time_since_epoch())
+      .count();
+}
+
+}  // namespace
+
 OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
                            std::shared_ptr<Parameters> parameters, bool use_intra_process)
     : node_(node),
@@ -58,6 +74,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
   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()) {
+      frame_timestamp_csv_logger_.reset();
+    }
+  }
 #if defined(USE_RK_HW_DECODER)
   jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
 #elif defined(USE_NV_HW_DECODER)
@@ -110,6 +138,10 @@ void OBCameraNode::clean() noexcept {
   std::lock_guard<decltype(device_lock_)> lock(device_lock_);
   RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
   is_running_.store(false);
+  if (frame_timestamp_csv_logger_) {
+    frame_timestamp_csv_logger_->shutdown();
+    frame_timestamp_csv_logger_.reset();
+  }
   RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
   if (tf_thread_ && tf_thread_->joinable()) {
     tf_thread_->join();
@@ -1247,6 +1279,8 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(depth_ae_roi_bottom_, "depth_ae_roi_bottom", -1);
 
   setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "device");
+  setAndGetNodeParameter<bool>(enable_frame_timestamp_csv_, "enable_frame_timestamp_csv", false);
+  setAndGetNodeParameter<std::string>(frame_timestamp_csv_file_, "frame_timestamp_csv_file", "");
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info.get());
   auto pid = device_info->pid();
@@ -1820,6 +1854,8 @@ 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();
   try {
     if (!tf_published_) {
       publishStaticTransforms();
@@ -1848,6 +1884,18 @@ 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) {
@@ -2201,6 +2249,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   image_msg->header.frame_id = frame_id;
   CHECK(image_publishers_.count(stream_index) > 0);
   saveImageToFile(stream_index, image, *image_msg);
+  if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
+      (stream_index == COLOR || stream_index == DEPTH)) {
+    frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(),
+                                                       getSteadyNowUs());
+  }
   image_publishers_[stream_index]->publish(std::move(image_msg));
   if (stream_index == COLOR && enable_color_undistortion_ &&
       color_undistortion_publisher_->get_subscription_count() > 0) {