Merge branch 'feat/imu-timestamp-csv' into v2/develop

orbbec_camera/CMakeLists.txt

@@ -159,6 +159,8 @@ set(SOURCE_FILES
   src/dynamic_params.cpp
   src/image_publisher.cpp
   src/frame_timestamp_csv_logger.cpp
+  src/imu_timestamp_csv_logger.cpp
+  src/timestamp_csv_logger.cpp
   src/ob_camera_node_driver.cpp
   src/ob_camera_node.cpp
   src/ob_lidar_node.cpp

orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h

@@ -21,8 +21,10 @@ namespace orbbec_camera {
 
 class FrameTimestampCsvLogger {
  public:
+  enum class OutputMode { SYNCED, COLOR, DEPTH };
+
   FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
-                          rclcpp::Logger logger);
+                          OutputMode output_mode, rclcpp::Logger logger);
 
   ~FrameTimestampCsvLogger() noexcept;
 
@@ -137,7 +139,7 @@ class FrameTimestampCsvLogger {
   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();
+  std::string csvHeader() const;
 
   void writerThreadMain();
   std::string csvFilePathForIndex(uint64_t file_index) const;
@@ -152,6 +154,7 @@ class FrameTimestampCsvLogger {
   std::atomic_bool csv_writer_failed_{false};
   bool queue_warning_active_ = false;
   std::string csv_file_path_;
+  OutputMode output_mode_;
   std::ofstream csv_stream_;
   std::thread writer_thread_;
   uint64_t csv_file_index_ = 0;

orbbec_camera/include/orbbec_camera/imu_timestamp_csv_logger.h

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <atomic>
+#include <condition_variable>
+#include <cstdint>
+#include <deque>
+#include <fstream>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <thread>
+
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+
+class ImuTimestampCsvLogger {
+ public:
+  enum class OutputMode { SYNCED, ACCEL, GYRO };
+
+  ImuTimestampCsvLogger(const std::string &frame_csv_file_path, OutputMode output_mode,
+                        rclcpp::Logger logger);
+
+  ~ImuTimestampCsvLogger() noexcept;
+
+  ImuTimestampCsvLogger(const ImuTimestampCsvLogger &) = delete;
+  ImuTimestampCsvLogger &operator=(const ImuTimestampCsvLogger &) = delete;
+
+  void recordFrameSet(const std::shared_ptr<ob::Frame> &accel_frame,
+                      const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
+                      std::optional<int64_t> publish_system_us);
+
+  void recordStandaloneFrame(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                             int64_t arrival_system_us, std::optional<int64_t> publish_system_us);
+
+  void shutdown();
+
+  bool enabled() const { return enabled_; }
+
+ private:
+  struct StreamState {
+    bool has_frame = false;
+    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;
+    std::optional<int64_t> publish_system_us;
+  };
+
+  struct PendingRow {
+    uint64_t row_id = 0;
+    StreamState accel;
+    StreamState gyro;
+  };
+
+  void recordFrames(const std::shared_ptr<ob::Frame> &accel_frame,
+                    const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
+                    std::optional<int64_t> publish_system_us);
+  static void populateStreamState(StreamState &state, const std::shared_ptr<ob::Frame> &frame,
+                                  int64_t arrival_system_us,
+                                  std::optional<int64_t> publish_system_us);
+
+  void enqueueCompletedRow(const PendingRow &row);
+  std::string serializeRow(const PendingRow &row) const;
+  static std::string serializeStreamColumns(const StreamState &state);
+  static std::string formatSecondsColumn(int64_t time_us);
+  static std::string formatOptionalSecondsColumn(const std::optional<int64_t> &value);
+  std::string csvHeader() const;
+
+  void writerThreadMain();
+  std::string csvFilePathForIndex(uint64_t file_index) const;
+  bool openCsvFile(uint64_t file_index);
+  bool rotateCsvFile();
+
+  rclcpp::Logger logger_;
+  bool enabled_ = false;
+  bool csv_enabled_ = false;
+  std::atomic_bool shutdown_requested_{false};
+  std::atomic_bool csv_writer_failed_{false};
+  bool queue_warning_active_ = false;
+  std::string frame_csv_file_path_;
+  OutputMode output_mode_;
+  std::ofstream csv_stream_;
+  std::thread writer_thread_;
+  uint64_t csv_file_index_ = 0;
+  uint64_t csv_rows_written_ = 0;
+
+  uint64_t next_row_id_ = 1;
+  std::deque<PendingRow> completed_rows_;
+
+  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

@@ -75,7 +75,7 @@
 #include "orbbec_camera/image_publisher.h"
 #include "orbbec_camera/fps_counter.hpp"
 #include "orbbec_camera/fps_delay_status.hpp"
-#include "orbbec_camera/frame_timestamp_csv_logger.h"
+#include "orbbec_camera/timestamp_csv_logger.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/header.hpp>
 #include <fcntl.h>
@@ -626,7 +626,8 @@ class OBCameraNode {
                        const std::shared_ptr<ob::Frame>& frame);
 
   void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
-                                       const std::shared_ptr<ob::Frame>& gryoframe);
+                                       const std::shared_ptr<ob::Frame>& gryoframe,
+                                       int64_t arrival_system_us);
 
   void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                              const stream_index_pair& stream_index);
@@ -1086,7 +1087,7 @@ class OBCameraNode {
   std::string time_domain_ = "global";  // device, system, global
   bool enable_frame_drop_log_ = false;
   std::string frame_timestamp_csv_file_;
-  std::unique_ptr<FrameTimestampCsvLogger> frame_timestamp_csv_logger_;
+  std::unique_ptr<TimestampCsvLogger> timestamp_csv_logger_;
   std::string exposure_range_mode_;
   std::string load_config_json_file_path_ = "";
   std::string export_config_json_file_path_ = "";

orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h

@@ -0,0 +1,73 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <atomic>
+#include <cstdint>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+
+class FrameTimestampCsvLogger;
+class ImuTimestampCsvLogger;
+
+class TimestampCsvLogger {
+ public:
+  struct Config {
+    bool frame_drop_log_enabled = false;
+    std::string csv_file_path;
+    bool frame_sync_enabled = false;
+    bool color_enabled = false;
+    bool depth_enabled = false;
+    bool imu_sync_enabled = false;
+    bool accel_enabled = false;
+    bool gyro_enabled = false;
+  };
+
+  TimestampCsvLogger(Config config, rclcpp::Logger logger);
+  ~TimestampCsvLogger() noexcept;
+
+  TimestampCsvLogger(const TimestampCsvLogger &) = delete;
+  TimestampCsvLogger &operator=(const TimestampCsvLogger &) = delete;
+
+  bool enabled() const;
+  bool imageEnabled() const;
+  bool imageStreamEnabled(OBStreamType stream_type) const;
+  bool syncedImuEnabled() const;
+  bool standaloneImuEnabled(OBStreamType stream_type) const;
+
+  void recordImageFrameSet(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 recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                             int64_t publish_system_us, int64_t publish_steady_us);
+  void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame);
+
+  void recordSyncedImu(const std::shared_ptr<ob::Frame> &accel_frame,
+                       const std::shared_ptr<ob::Frame> &gyro_frame, int64_t arrival_system_us,
+                       std::optional<int64_t> publish_system_us);
+  void recordStandaloneImu(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                           int64_t arrival_system_us, std::optional<int64_t> publish_system_us);
+
+  void shutdown() noexcept;
+
+ private:
+  FrameTimestampCsvLogger *imageLoggerForStream(OBStreamType stream_type) const;
+  ImuTimestampCsvLogger *standaloneImuLoggerForStream(OBStreamType stream_type) const;
+
+  rclcpp::Logger logger_;
+  std::atomic_bool shutdown_requested_{false};
+  std::unique_ptr<FrameTimestampCsvLogger> synced_image_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> color_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> depth_logger_;
+  std::unique_ptr<ImuTimestampCsvLogger> synced_imu_logger_;
+  std::unique_ptr<ImuTimestampCsvLogger> accel_logger_;
+  std::unique_ptr<ImuTimestampCsvLogger> gyro_logger_;
+};
+
+}  // namespace orbbec_camera

orbbec_camera/src/frame_timestamp_csv_logger.cpp

@@ -35,25 +35,26 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
 
 FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
                                                  const std::string &csv_file_path,
-                                                 rclcpp::Logger logger)
+                                                 OutputMode output_mode, rclcpp::Logger logger)
     : logger_(std::move(logger)),
       enabled_(drop_log_enabled || !csv_file_path.empty()),
       csv_enabled_(!csv_file_path.empty()),
       drop_log_enabled_(drop_log_enabled),
-      csv_file_path_(csv_file_path) {
+      csv_file_path_(csv_file_path),
+      output_mode_(output_mode) {
   if (!enabled_) {
     return;
   }
 
   if (csv_enabled_) {
     try {
-      auto path = std::filesystem::path(csv_file_path_);
+      auto path = std::filesystem::path(csvFilePathForIndex(0));
       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());
+                                       << csvFilePathForIndex(0) << ": " << e.what());
       csv_enabled_ = false;
       csv_writer_failed_ = true;
     }
@@ -74,7 +75,7 @@ FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
   if (enabled_) {
     RCLCPP_INFO_STREAM(logger_,
                        "Frame timestamp logger enabled: csv_file="
-                           << (csv_enabled_ ? csv_file_path_ : "disabled")
+                           << (csv_enabled_ ? csvFilePathForIndex(0) : "disabled")
                            << " frame_drop_log=" << (drop_log_enabled_ ? "enabled" : "disabled"));
   }
 }
@@ -90,6 +91,9 @@ void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &c
   if (!enabled_) {
     return;
   }
+  if (output_mode_ != OutputMode::SYNCED) {
+    return;
+  }
   recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us,
                          track_color, track_depth, color_image_publish_expected,
                          depth_image_publish_expected);
@@ -100,7 +104,9 @@ 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)) {
+  if (!enabled_ || !frame || !isTrackedStream(stream_type) ||
+      (stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) ||
+      (stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) {
     return;
   }
   recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -479,6 +485,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);
+  }
   std::ostringstream ss;
   ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
   return ss.str();
@@ -529,9 +541,9 @@ std::string FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional
   return std::to_string(*value);
 }
 
-std::string FrameTimestampCsvLogger::csvHeader() {
+std::string FrameTimestampCsvLogger::csvHeader() const {
   std::ostringstream ss;
-  for (const auto *prefix : {"color", "depth"}) {
+  const auto append_stream_header = [&ss](const char *prefix) {
     ss << prefix << "_sdk_frame_index,";
     ss << prefix << "_hardware_frame_number,";
     ss << prefix << "_sensor_ts_sec,";
@@ -547,9 +559,15 @@ std::string FrameTimestampCsvLogger::csvHeader() {
     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 << ",";
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) {
-    }
+    append_stream_header("color");
+  }
+  if (output_mode_ == OutputMode::SYNCED) {
+    ss << ",";
+  }
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
+    append_stream_header("depth");
   }
   return ss.str();
 }
@@ -621,13 +639,19 @@ void FrameTimestampCsvLogger::writerThreadMain() {
 }
 
 std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
-  if (file_index == 0) {
+  const std::filesystem::path original_path(csv_file_path_);
-    return csv_file_path_;
+  std::string suffix;
+  if (output_mode_ == OutputMode::COLOR) {
+    suffix = "_color";
+  } else if (output_mode_ == OutputMode::DEPTH) {
+    suffix = "_depth";
   }
 
-  const std::filesystem::path original_path(csv_file_path_);
+  auto indexed_filename = original_path.stem().string() + suffix;
-  const auto indexed_filename = original_path.stem().string() + "_" + std::to_string(file_index) +
+  if (file_index != 0) {
-                                original_path.extension().string();
+    indexed_filename += "_" + std::to_string(file_index);
+  }
+  indexed_filename += original_path.extension().string();
   return (original_path.parent_path() / indexed_filename).string();
 }
 

orbbec_camera/src/imu_timestamp_csv_logger.cpp

@@ -0,0 +1,354 @@
+#include "orbbec_camera/imu_timestamp_csv_logger.h"
+
+#include <algorithm>
+#include <chrono>
+#include <filesystem>
+#include <iomanip>
+#include <sstream>
+#include <utility>
+#include <vector>
+
+namespace orbbec_camera {
+namespace {
+
+constexpr size_t kCompletedQueueSoftLimit = 1000;
+constexpr size_t kFlushBatchSize = 100;
+// The header occupies the first row, leaving 1,024,575 rows for IMU data.
+constexpr uint64_t kMaxCsvRowsPerFileIncludingHeader = 1'024'576;
+constexpr auto kFlushInterval = std::chrono::seconds(1);
+
+}  // namespace
+
+ImuTimestampCsvLogger::ImuTimestampCsvLogger(const std::string &frame_csv_file_path,
+                                             OutputMode output_mode, rclcpp::Logger logger)
+    : logger_(std::move(logger)),
+      enabled_(!frame_csv_file_path.empty()),
+      csv_enabled_(!frame_csv_file_path.empty()),
+      frame_csv_file_path_(frame_csv_file_path),
+      output_mode_(output_mode) {
+  if (!enabled_) {
+    return;
+  }
+
+  if (csv_enabled_) {
+    try {
+      const auto path = std::filesystem::path(csvFilePathForIndex(0));
+      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 IMU timestamp CSV path "
+                                       << csvFilePathForIndex(0) << ": " << e.what());
+      csv_enabled_ = false;
+      csv_writer_failed_ = true;
+    }
+  }
+
+  if (csv_enabled_ && !openCsvFile(0)) {
+    csv_enabled_ = false;
+    csv_writer_failed_ = true;
+  }
+
+  enabled_ = csv_enabled_;
+  if (csv_enabled_) {
+    writer_thread_ = std::thread([this]() { writerThreadMain(); });
+  }
+
+  if (enabled_) {
+    RCLCPP_INFO_STREAM(logger_,
+                       "IMU timestamp logger enabled: csv_file=" << csvFilePathForIndex(0));
+  }
+}
+
+ImuTimestampCsvLogger::~ImuTimestampCsvLogger() noexcept { shutdown(); }
+
+void ImuTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &accel_frame,
+                                           const std::shared_ptr<ob::Frame> &gyro_frame,
+                                           int64_t arrival_system_us,
+                                           std::optional<int64_t> publish_system_us) {
+  if (!enabled_ || output_mode_ != OutputMode::SYNCED || (!accel_frame && !gyro_frame)) {
+    return;
+  }
+  recordFrames(accel_frame, gyro_frame, arrival_system_us, publish_system_us);
+}
+
+void ImuTimestampCsvLogger::recordStandaloneFrame(OBStreamType stream_type,
+                                                  const std::shared_ptr<ob::Frame> &frame,
+                                                  int64_t arrival_system_us,
+                                                  std::optional<int64_t> publish_system_us) {
+  if (!enabled_ || !frame) {
+    return;
+  }
+  if (stream_type == OB_STREAM_ACCEL && output_mode_ == OutputMode::ACCEL) {
+    recordFrames(frame, nullptr, arrival_system_us, publish_system_us);
+  } else if (stream_type == OB_STREAM_GYRO && output_mode_ == OutputMode::GYRO) {
+    recordFrames(nullptr, frame, arrival_system_us, publish_system_us);
+  }
+}
+
+void ImuTimestampCsvLogger::shutdown() {
+  if (!enabled_) {
+    return;
+  }
+
+  {
+    std::lock_guard<std::mutex> state_lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+    shutdown_requested_ = true;
+  }
+
+  completed_rows_cv_.notify_all();
+  if (writer_thread_.joinable()) {
+    writer_thread_.join();
+  }
+
+  if (csv_stream_.is_open()) {
+    csv_stream_.flush();
+    csv_stream_.close();
+  }
+}
+
+void ImuTimestampCsvLogger::recordFrames(const std::shared_ptr<ob::Frame> &accel_frame,
+                                         const std::shared_ptr<ob::Frame> &gyro_frame,
+                                         int64_t arrival_system_us,
+                                         std::optional<int64_t> publish_system_us) {
+  std::lock_guard<std::mutex> lock(state_mutex_);
+  if (shutdown_requested_) {
+    return;
+  }
+
+  PendingRow row;
+  row.row_id = next_row_id_++;
+  if (accel_frame) {
+    populateStreamState(row.accel, accel_frame, arrival_system_us, publish_system_us);
+  }
+  if (gyro_frame) {
+    populateStreamState(row.gyro, gyro_frame, arrival_system_us, publish_system_us);
+  }
+  enqueueCompletedRow(row);
+}
+
+void ImuTimestampCsvLogger::populateStreamState(StreamState &state,
+                                                const std::shared_ptr<ob::Frame> &frame,
+                                                int64_t arrival_system_us,
+                                                std::optional<int64_t> publish_system_us) {
+  state.has_frame = true;
+  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.publish_system_us = publish_system_us;
+}
+
+void ImuTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) {
+  if (!csv_enabled_ || csv_writer_failed_) {
+    return;
+  }
+
+  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_, "IMU timestamp CSV queue size exceeded " << kCompletedQueueSoftLimit << " rows");
+      queue_warning_active_ = true;
+    }
+  } else {
+    queue_warning_active_ = false;
+  }
+  completed_rows_cv_.notify_one();
+}
+
+std::string ImuTimestampCsvLogger::serializeRow(const PendingRow &row) const {
+  if (output_mode_ == OutputMode::ACCEL) {
+    return serializeStreamColumns(row.accel);
+  }
+  if (output_mode_ == OutputMode::GYRO) {
+    return serializeStreamColumns(row.gyro);
+  }
+  std::ostringstream ss;
+  ss << serializeStreamColumns(row.accel) << "," << serializeStreamColumns(row.gyro);
+  return ss.str();
+}
+
+std::string ImuTimestampCsvLogger::serializeStreamColumns(const StreamState &state) {
+  std::vector<std::string> fields(5, "");
+  if (state.has_frame) {
+    fields[0] = formatSecondsColumn(state.device_ts_us);
+    fields[1] = formatSecondsColumn(state.global_ts_us);
+    fields[2] = formatSecondsColumn(state.sdk_system_ts_us);
+    fields[3] = formatSecondsColumn(state.arrival_system_us);
+    fields[4] = formatOptionalSecondsColumn(state.publish_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 ImuTimestampCsvLogger::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 ImuTimestampCsvLogger::formatOptionalSecondsColumn(
+    const std::optional<int64_t> &value) {
+  if (!value.has_value()) {
+    return "";
+  }
+  return formatSecondsColumn(*value);
+}
+
+std::string ImuTimestampCsvLogger::csvHeader() const {
+  std::ostringstream ss;
+  const auto append_stream_header = [&ss](const char *prefix) {
+    ss << prefix << "_device_ts_sec,";
+    ss << prefix << "_global_ts_sec,";
+    ss << prefix << "_system_ts_sec,";
+    ss << prefix << "_arrival_system_ts_sec,";
+    ss << prefix << "_publish_system_ts_sec";
+  };
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::ACCEL) {
+    append_stream_header("accel");
+  }
+  if (output_mode_ == OutputMode::SYNCED) {
+    ss << ",";
+  }
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::GYRO) {
+    append_stream_header("gyro");
+  }
+  return ss.str();
+}
+
+void ImuTimestampCsvLogger::writerThreadMain() {
+  if (!csv_enabled_ || csv_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_);
+    }
+
+    std::stable_sort(rows_to_write.begin(), rows_to_write.end(),
+                     [](const auto &lhs, const auto &rhs) { return lhs.row_id < rhs.row_id; });
+
+    for (const auto &row : rows_to_write) {
+      if (csv_rows_written_ >= kMaxCsvRowsPerFileIncludingHeader) {
+        if (!rotateCsvFile()) {
+          csv_writer_failed_ = true;
+          break;
+        }
+        rows_since_flush = 0;
+        last_flush = std::chrono::steady_clock::now();
+      }
+
+      if (!csv_stream_.is_open()) {
+        csv_writer_failed_ = true;
+        break;
+      }
+
+      csv_stream_ << serializeRow(row) << "\n";
+      if (!csv_stream_) {
+        RCLCPP_ERROR_STREAM(logger_, "Failed to write IMU timestamp CSV file: "
+                                         << csvFilePathForIndex(csv_file_index_));
+        csv_writer_failed_ = true;
+        break;
+      }
+      ++csv_rows_written_;
+      ++rows_since_flush;
+    }
+
+    if (csv_writer_failed_) {
+      break;
+    }
+
+    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;
+    }
+  }
+}
+
+std::string ImuTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
+  const std::filesystem::path original_path(frame_csv_file_path_);
+  std::string suffix;
+  if (output_mode_ == OutputMode::SYNCED) {
+    suffix = "_imu";
+  } else if (output_mode_ == OutputMode::ACCEL) {
+    suffix = "_accel";
+  } else {
+    suffix = "_gyro";
+  }
+  auto indexed_filename = original_path.stem().string() + suffix;
+  if (file_index != 0) {
+    indexed_filename += "_" + std::to_string(file_index);
+  }
+  indexed_filename += original_path.extension().string();
+  return (original_path.parent_path() / indexed_filename).string();
+}
+
+bool ImuTimestampCsvLogger::openCsvFile(uint64_t file_index) {
+  const auto file_path = csvFilePathForIndex(file_index);
+  csv_stream_.clear();
+  csv_stream_.open(file_path, std::ios::out | std::ios::trunc);
+  if (!csv_stream_.is_open()) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to open IMU timestamp CSV file: " << file_path);
+    return false;
+  }
+
+  csv_stream_ << csvHeader() << "\n";
+  csv_stream_.flush();
+  if (!csv_stream_) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to write IMU timestamp CSV header: " << file_path);
+    csv_stream_.close();
+    return false;
+  }
+
+  csv_file_index_ = file_index;
+  csv_rows_written_ = 1;
+  return true;
+}
+
+bool ImuTimestampCsvLogger::rotateCsvFile() {
+  if (csv_stream_.is_open()) {
+    csv_stream_.flush();
+    csv_stream_.close();
+  }
+
+  const auto next_file_index = csv_file_index_ + 1;
+  if (!openCsvFile(next_file_index)) {
+    return false;
+  }
+
+  RCLCPP_INFO_STREAM(logger_, "IMU timestamp CSV reached " << kMaxCsvRowsPerFileIncludingHeader
+                                                           << " rows; continuing in "
+                                                           << csvFilePathForIndex(csv_file_index_));
+  return true;
+}
+
+}  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -736,10 +736,19 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   setupTopics();
 
   if (enable_frame_drop_log_ || !frame_timestamp_csv_file_.empty()) {
-    frame_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
+    TimestampCsvLogger::Config timestamp_config;
-        enable_frame_drop_log_, frame_timestamp_csv_file_, logger_);
+    timestamp_config.frame_drop_log_enabled = enable_frame_drop_log_;
-    if (!frame_timestamp_csv_logger_->enabled()) {
+    timestamp_config.csv_file_path = frame_timestamp_csv_file_;
-      frame_timestamp_csv_logger_.reset();
+    timestamp_config.frame_sync_enabled = enable_frame_sync_;
+    timestamp_config.color_enabled = enable_stream_[COLOR];
+    timestamp_config.depth_enabled = enable_stream_[DEPTH];
+    timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
+    timestamp_config.accel_enabled = enable_stream_[ACCEL];
+    timestamp_config.gyro_enabled = enable_stream_[GYRO];
+    timestamp_csv_logger_ =
+        std::make_unique<TimestampCsvLogger>(std::move(timestamp_config), logger_);
+    if (!timestamp_csv_logger_->enabled()) {
+      timestamp_csv_logger_.reset();
     }
   }
 
@@ -809,15 +818,6 @@ void OBCameraNode::clean() noexcept {
   is_running_.store(false);
   is_camera_node_initialized_.store(false);
 
-  try {
-    if (frame_timestamp_csv_logger_) {
-      frame_timestamp_csv_logger_->shutdown();
-      frame_timestamp_csv_logger_.reset();
-    }
-  } catch (...) {
-    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down frame timestamp CSV logger");
-  }
-
   // Stop diagnostic timer and updater first BEFORE acquiring device_lock to prevent deadlock
   try {
     if (diagnostic_timer_) {
@@ -877,6 +877,11 @@ void OBCameraNode::clean() noexcept {
     RCLCPP_DEBUG_STREAM(logger_, "Exception while stopping streams");
   }
 
+  if (timestamp_csv_logger_) {
+    timestamp_csv_logger_->shutdown();
+    timestamp_csv_logger_.reset();
+  }
+
   // Clean up d2c_viewer_ before cleaning buffers
   RCLCPP_DEBUG_STREAM(logger_, "Clean d2c_viewer");
   try {
@@ -4200,8 +4205,14 @@ void OBCameraNode::startIMUSyncStream() {
       auto frameSet = frame->as<ob::FrameSet>();
       auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
       auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
+      const bool log_imu_timestamps = is_camera_node_initialized_.load() && rclcpp::ok() &&
+                                      timestamp_csv_logger_ &&
+                                      timestamp_csv_logger_->syncedImuEnabled();
+      const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
       if (aFrame && gFrame) {
-        onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
+        onNewIMUFrameSyncOutputCallback(aFrame, gFrame, arrival_system_us);
+      } else if (log_imu_timestamps && (aFrame || gFrame)) {
+        timestamp_csv_logger_->recordSyncedImu(aFrame, gFrame, arrival_system_us, std::nullopt);
       }
     });
 
@@ -6334,7 +6345,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
   if (frame_set == nullptr) {
     return;
   }
-  if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) {
+  if (timestamp_csv_logger_ && timestamp_csv_logger_->imageEnabled()) {
     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);
@@ -6344,7 +6355,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
     const bool color_publish_expected = track_color;
     const bool depth_publish_expected = track_depth;
 
-    frame_timestamp_csv_logger_->recordFrameSet(
+    timestamp_csv_logger_->recordImageFrameSet(
         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);
@@ -6819,7 +6830,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
       target_buffer_size = &rgb_buffer_size_;
     }
     if (video_frame->getDataSize() > *target_buffer_size) {
-      delete[](*target_buffer);
+      delete[] (*target_buffer);
       *target_buffer_size = video_frame->getDataSize();
       *target_buffer = new uint8_t[*target_buffer_size];
       buffer = *target_buffer;
@@ -6871,10 +6882,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   if (frame == nullptr) {
     return;
   }
+  const bool log_image_timestamps =
+      timestamp_csv_logger_ && timestamp_csv_logger_->imageStreamEnabled(stream_index.first);
   const auto record_image_publish_skipped = [&]() {
-    if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
+    if (log_image_timestamps) {
-        (stream_index == COLOR || stream_index == DEPTH)) {
+      timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame);
-      frame_timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame);
     }
   };
   CHECK_NOTNULL(image_publishers_[stream_index]);
@@ -6991,10 +7003,9 @@ 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 && frame_timestamp_csv_logger_ &&
+    if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) {
-        frame_timestamp_csv_logger_->enabled()) {
+      timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
-      frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
+                                                   getSteadyNowUs());
-                                                         getSystemNowUs(), getSteadyNowUs());
     }
     publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
     if (!has_raw_image_subscriber && stream_index == COLOR) {
@@ -7080,10 +7091,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       record_image_publish_skipped();
       return;
     }
-    if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
+    if (log_image_timestamps) {
-        (stream_index == COLOR || stream_index == DEPTH)) {
+      timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
-      frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
+                                                   getSteadyNowUs());
-                                                         getSystemNowUs(), getSteadyNowUs());
     }
     if (stream_index == COLOR) {
       fps_delay_status_color_->tick(frame_timestamp);
@@ -7270,11 +7280,20 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
 }
 
 void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame> &accelframe,
-                                                   const std::shared_ptr<ob::Frame> &gryoframe) {
+                                                   const std::shared_ptr<ob::Frame> &gryoframe,
+                                                   int64_t arrival_system_us) {
   if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
     return;
   }
+  const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
+    if (arrival_system_us != 0 && timestamp_csv_logger_ &&
+        timestamp_csv_logger_->syncedImuEnabled()) {
+      timestamp_csv_logger_->recordSyncedImu(accelframe, gryoframe, arrival_system_us,
+                                             publish_system_us);
+    }
+  };
   if (!imu_gyro_accel_publisher_) {
+    record_timestamps(std::nullopt);
     RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized");
     return;
   }
@@ -7282,6 +7301,7 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
   has_subscriber = has_subscriber || imu_info_publishers_[GYRO]->get_subscription_count() > 0;
   has_subscriber = has_subscriber || imu_info_publishers_[ACCEL]->get_subscription_count() > 0;
   if (!has_subscriber) {
+    record_timestamps(std::nullopt);
     return;
   }
   auto imu_msg = sensor_msgs::msg::Imu();
@@ -7315,7 +7335,9 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
   imu_msg.linear_acceleration.y = accelData.y - accel_info.bias[1];
   imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];
 
+  const auto publish_system_us = getSystemNowUs();
   imu_gyro_accel_publisher_->publish(imu_msg);
+  record_timestamps(publish_system_us);
 }
 
 void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
@@ -7323,7 +7345,17 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
   if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
     return;
   }
+  const bool log_imu_timestamps =
+      timestamp_csv_logger_ && timestamp_csv_logger_->standaloneImuEnabled(stream_index.first);
+  const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
+  const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
+    if (log_imu_timestamps) {
+      timestamp_csv_logger_->recordStandaloneImu(stream_index.first, frame, arrival_system_us,
+                                                 publish_system_us);
+    }
+  };
   if (!imu_publishers_.count(stream_index)) {
+    record_timestamps(std::nullopt);
     RCLCPP_ERROR_STREAM(logger_,
                         "stream " << stream_name_[stream_index] << " publisher not initialized");
     return;
@@ -7332,6 +7364,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
   has_subscriber =
       has_subscriber || imu_info_publishers_[stream_index]->get_subscription_count() > 0;
   if (!has_subscriber) {
+    record_timestamps(std::nullopt);
     return;
   }
   auto imu_msg = sensor_msgs::msg::Imu();
@@ -7358,10 +7391,13 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
     imu_msg.linear_acceleration.y = data.y - imu_info.bias[1];
     imu_msg.linear_acceleration.z = data.z - imu_info.bias[2];
   } else {
+    record_timestamps(std::nullopt);
     RCLCPP_ERROR(logger_, "Unsupported IMU frame type");
     return;
   }
+  const auto publish_system_us = getSystemNowUs();
   imu_publishers_[stream_index]->publish(imu_msg);
+  record_timestamps(publish_system_us);
 }
 
 void OBCameraNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
@@ -8456,9 +8492,9 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
         if (request->filter_param.size() > 1) {
           temporal_filter->setDiffScale(request->filter_param[0]);
           temporal_filter->setWeight(request->filter_param[1]);
-          RCLCPP_INFO_STREAM(logger_, "Set TemporalFilter params: "
+          RCLCPP_INFO_STREAM(
-                                          << "\ndiff_scale:" << request->filter_param[0]
+              logger_, "Set TemporalFilter params: " << "\ndiff_scale:" << request->filter_param[0]
-                                          << "\nweight:" << request->filter_param[1]);
+                                                     << "\nweight:" << request->filter_param[1]);
           temporal_filter_diff_threshold_ = request->filter_param[0];
           temporal_filter_weight_ = request->filter_param[1];
         } else {

orbbec_camera/src/timestamp_csv_logger.cpp

@@ -0,0 +1,194 @@
+#include "orbbec_camera/timestamp_csv_logger.h"
+
+#include "orbbec_camera/frame_timestamp_csv_logger.h"
+#include "orbbec_camera/imu_timestamp_csv_logger.h"
+
+#include <exception>
+#include <utility>
+
+namespace orbbec_camera {
+
+TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger)
+    : logger_(std::move(logger)) {
+  const auto create_image_logger = [this, &config](FrameTimestampCsvLogger::OutputMode mode) {
+    auto timestamp_logger = std::make_unique<FrameTimestampCsvLogger>(
+        config.frame_drop_log_enabled, config.csv_file_path, mode, logger_);
+    if (!timestamp_logger->enabled()) {
+      timestamp_logger.reset();
+    }
+    return timestamp_logger;
+  };
+
+  if (config.frame_sync_enabled && (config.color_enabled || config.depth_enabled)) {
+    synced_image_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::SYNCED);
+  } else {
+    if (config.color_enabled) {
+      color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::COLOR);
+    }
+    if (config.depth_enabled) {
+      depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH);
+    }
+  }
+
+  if (config.csv_file_path.empty()) {
+    return;
+  }
+
+  const auto create_imu_logger = [this, &config](ImuTimestampCsvLogger::OutputMode mode) {
+    auto timestamp_logger =
+        std::make_unique<ImuTimestampCsvLogger>(config.csv_file_path, mode, logger_);
+    if (!timestamp_logger->enabled()) {
+      timestamp_logger.reset();
+    }
+    return timestamp_logger;
+  };
+
+  if (config.imu_sync_enabled) {
+    synced_imu_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::SYNCED);
+  } else {
+    if (config.accel_enabled) {
+      accel_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::ACCEL);
+    }
+    if (config.gyro_enabled) {
+      gyro_logger_ = create_imu_logger(ImuTimestampCsvLogger::OutputMode::GYRO);
+    }
+  }
+}
+
+TimestampCsvLogger::~TimestampCsvLogger() noexcept { shutdown(); }
+
+bool TimestampCsvLogger::enabled() const {
+  return imageEnabled() || syncedImuEnabled() || standaloneImuEnabled(OB_STREAM_ACCEL) ||
+         standaloneImuEnabled(OB_STREAM_GYRO);
+}
+
+bool TimestampCsvLogger::imageEnabled() const {
+  return (synced_image_logger_ && synced_image_logger_->enabled()) ||
+         (color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled());
+}
+
+bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const {
+  const auto *timestamp_logger = imageLoggerForStream(stream_type);
+  return timestamp_logger && timestamp_logger->enabled();
+}
+
+bool TimestampCsvLogger::syncedImuEnabled() const {
+  return synced_imu_logger_ && synced_imu_logger_->enabled();
+}
+
+bool TimestampCsvLogger::standaloneImuEnabled(OBStreamType stream_type) const {
+  const auto *timestamp_logger = standaloneImuLoggerForStream(stream_type);
+  return timestamp_logger && timestamp_logger->enabled();
+}
+
+void TimestampCsvLogger::recordImageFrameSet(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 (synced_image_logger_) {
+    synced_image_logger_->recordFrameSet(
+        color_frame, depth_frame, arrival_system_us, arrival_steady_us, track_color, track_depth,
+        color_image_publish_expected, depth_image_publish_expected);
+    return;
+  }
+
+  if (track_color && color_logger_) {
+    color_logger_->recordStandaloneFrameArrival(OB_STREAM_COLOR, color_frame, arrival_system_us,
+                                                arrival_steady_us, color_image_publish_expected);
+  }
+  if (track_depth && depth_logger_) {
+    depth_logger_->recordStandaloneFrameArrival(OB_STREAM_DEPTH, depth_frame, arrival_system_us,
+                                                arrival_steady_us, depth_image_publish_expected);
+  }
+}
+
+void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type,
+                                               const std::shared_ptr<ob::Frame> &frame,
+                                               int64_t publish_system_us,
+                                               int64_t publish_steady_us) {
+  auto *timestamp_logger = imageLoggerForStream(stream_type);
+  if (timestamp_logger) {
+    timestamp_logger->recordPreImagePublish(stream_type, frame, publish_system_us,
+                                            publish_steady_us);
+  }
+}
+
+void TimestampCsvLogger::recordImagePublishSkipped(OBStreamType stream_type,
+                                                   const std::shared_ptr<ob::Frame> &frame) {
+  auto *timestamp_logger = imageLoggerForStream(stream_type);
+  if (timestamp_logger) {
+    timestamp_logger->recordImagePublishSkipped(stream_type, frame);
+  }
+}
+
+void TimestampCsvLogger::recordSyncedImu(const std::shared_ptr<ob::Frame> &accel_frame,
+                                         const std::shared_ptr<ob::Frame> &gyro_frame,
+                                         int64_t arrival_system_us,
+                                         std::optional<int64_t> publish_system_us) {
+  if (synced_imu_logger_) {
+    synced_imu_logger_->recordFrameSet(accel_frame, gyro_frame, arrival_system_us,
+                                       publish_system_us);
+  }
+}
+
+void TimestampCsvLogger::recordStandaloneImu(OBStreamType stream_type,
+                                             const std::shared_ptr<ob::Frame> &frame,
+                                             int64_t arrival_system_us,
+                                             std::optional<int64_t> publish_system_us) {
+  auto *timestamp_logger = standaloneImuLoggerForStream(stream_type);
+  if (timestamp_logger) {
+    timestamp_logger->recordStandaloneFrame(stream_type, frame, arrival_system_us,
+                                            publish_system_us);
+  }
+}
+
+void TimestampCsvLogger::shutdown() noexcept {
+  if (shutdown_requested_.exchange(true)) {
+    return;
+  }
+
+  const auto shutdown_logger = [this](auto &timestamp_logger, const char *name) {
+    if (!timestamp_logger) {
+      return;
+    }
+    try {
+      timestamp_logger->shutdown();
+    } catch (const std::exception &e) {
+      RCLCPP_WARN_STREAM(logger_, "Exception while shutting down " << name << ": " << e.what());
+    } catch (...) {
+      RCLCPP_WARN_STREAM(logger_, "Unknown exception while shutting down " << name);
+    }
+  };
+
+  shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger");
+  shutdown_logger(color_logger_, "color timestamp CSV logger");
+  shutdown_logger(depth_logger_, "depth 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");
+}
+
+FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType stream_type) const {
+  if (stream_type == OB_STREAM_COLOR) {
+    return synced_image_logger_ ? synced_image_logger_.get() : color_logger_.get();
+  }
+  if (stream_type == OB_STREAM_DEPTH) {
+    return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get();
+  }
+  return nullptr;
+}
+
+ImuTimestampCsvLogger *TimestampCsvLogger::standaloneImuLoggerForStream(
+    OBStreamType stream_type) const {
+  if (stream_type == OB_STREAM_ACCEL) {
+    return accel_logger_.get();
+  }
+  if (stream_type == OB_STREAM_GYRO) {
+    return gyro_logger_.get();
+  }
+  return nullptr;
+}
+
+}  // namespace orbbec_camera