refactor: centralize timestamp CSV logger management

orbbec_camera/CMakeLists.txt

@@ -159,6 +159,7 @@ set(SOURCE_FILES
   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/ob_camera_node.h

@@ -70,8 +70,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 "orbbec_camera/imu_timestamp_csv_logger.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/header.hpp>
 #include <fcntl.h>
@@ -1023,12 +1022,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::unique_ptr<FrameTimestampCsvLogger> color_timestamp_csv_logger_;
-  std::unique_ptr<FrameTimestampCsvLogger> depth_timestamp_csv_logger_;
-  std::unique_ptr<ImuTimestampCsvLogger> imu_timestamp_csv_logger_;
-  std::unique_ptr<ImuTimestampCsvLogger> accel_timestamp_csv_logger_;
-  std::unique_ptr<ImuTimestampCsvLogger> gyro_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/ob_camera_node.cpp

@@ -738,54 +738,19 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   setupTopics();
 
   if (enable_frame_drop_log_ || !frame_timestamp_csv_file_.empty()) {
-    if (enable_frame_sync_ && (enable_stream_[COLOR] || enable_stream_[DEPTH])) {
+    TimestampCsvLogger::Config timestamp_config;
-      frame_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
+    timestamp_config.frame_drop_log_enabled = enable_frame_drop_log_;
-          enable_frame_drop_log_, frame_timestamp_csv_file_,
+    timestamp_config.csv_file_path = frame_timestamp_csv_file_;
-          FrameTimestampCsvLogger::OutputMode::SYNCED, logger_);
+    timestamp_config.frame_sync_enabled = enable_frame_sync_;
-      if (!frame_timestamp_csv_logger_->enabled()) {
+    timestamp_config.color_enabled = enable_stream_[COLOR];
-        frame_timestamp_csv_logger_.reset();
+    timestamp_config.depth_enabled = enable_stream_[DEPTH];
-      }
+    timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
-    } else {
+    timestamp_config.accel_enabled = enable_stream_[ACCEL];
-      if (enable_stream_[COLOR]) {
+    timestamp_config.gyro_enabled = enable_stream_[GYRO];
-        color_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
+    timestamp_csv_logger_ =
-            enable_frame_drop_log_, frame_timestamp_csv_file_,
+        std::make_unique<TimestampCsvLogger>(std::move(timestamp_config), logger_);
-            FrameTimestampCsvLogger::OutputMode::COLOR, logger_);
+    if (!timestamp_csv_logger_->enabled()) {
-        if (!color_timestamp_csv_logger_->enabled()) {
+      timestamp_csv_logger_.reset();
-          color_timestamp_csv_logger_.reset();
-        }
-      }
-      if (enable_stream_[DEPTH]) {
-        depth_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
-            enable_frame_drop_log_, frame_timestamp_csv_file_,
-            FrameTimestampCsvLogger::OutputMode::DEPTH, logger_);
-        if (!depth_timestamp_csv_logger_->enabled()) {
-          depth_timestamp_csv_logger_.reset();
-        }
-      }
-    }
-    if (!frame_timestamp_csv_file_.empty() && (enable_stream_[ACCEL] || enable_stream_[GYRO])) {
-      if (enable_sync_output_accel_gyro_) {
-        imu_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
-            frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::SYNCED, logger_);
-        if (!imu_timestamp_csv_logger_->enabled()) {
-          imu_timestamp_csv_logger_.reset();
-        }
-      } else {
-        if (enable_stream_[ACCEL]) {
-          accel_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
-              frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::ACCEL, logger_);
-          if (!accel_timestamp_csv_logger_->enabled()) {
-            accel_timestamp_csv_logger_.reset();
-          }
-        }
-        if (enable_stream_[GYRO]) {
-          gyro_timestamp_csv_logger_ = std::make_unique<ImuTimestampCsvLogger>(
-              frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::GYRO, logger_);
-          if (!gyro_timestamp_csv_logger_->enabled()) {
-            gyro_timestamp_csv_logger_.reset();
-          }
-        }
-      }
     }
   }
 
@@ -855,53 +820,9 @@ void OBCameraNode::clean() noexcept {
   is_running_.store(false);
   is_camera_node_initialized_.store(false);
 
-  try {
+  if (timestamp_csv_logger_) {
-    if (frame_timestamp_csv_logger_) {
+    timestamp_csv_logger_->shutdown();
-      frame_timestamp_csv_logger_->shutdown();
+    timestamp_csv_logger_.reset();
-      frame_timestamp_csv_logger_.reset();
-    }
-  } catch (...) {
-    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down frame timestamp CSV logger");
-  }
-  try {
-    if (color_timestamp_csv_logger_) {
-      color_timestamp_csv_logger_->shutdown();
-      color_timestamp_csv_logger_.reset();
-    }
-  } catch (...) {
-    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down color timestamp CSV logger");
-  }
-  try {
-    if (depth_timestamp_csv_logger_) {
-      depth_timestamp_csv_logger_->shutdown();
-      depth_timestamp_csv_logger_.reset();
-    }
-  } catch (...) {
-    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down depth timestamp CSV logger");
-  }
-  try {
-    if (imu_timestamp_csv_logger_) {
-      imu_timestamp_csv_logger_->shutdown();
-      imu_timestamp_csv_logger_.reset();
-    }
-  } catch (...) {
-    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down IMU timestamp CSV logger");
-  }
-  try {
-    if (accel_timestamp_csv_logger_) {
-      accel_timestamp_csv_logger_->shutdown();
-      accel_timestamp_csv_logger_.reset();
-    }
-  } catch (...) {
-    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down accel timestamp CSV logger");
-  }
-  try {
-    if (gyro_timestamp_csv_logger_) {
-      gyro_timestamp_csv_logger_->shutdown();
-      gyro_timestamp_csv_logger_.reset();
-    }
-  } catch (...) {
-    RCLCPP_WARN_STREAM(logger_, "Exception while shutting down gyro timestamp CSV logger");
   }
 
   // Stop diagnostic timer and updater first BEFORE acquiring device_lock to prevent deadlock
@@ -4214,13 +4135,13 @@ void OBCameraNode::startIMUSyncStream() {
       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() &&
-                                      imu_timestamp_csv_logger_ &&
+                                      timestamp_csv_logger_ &&
-                                      imu_timestamp_csv_logger_->enabled();
+                                      timestamp_csv_logger_->syncedImuEnabled();
       const auto arrival_system_us = log_imu_timestamps ? getSystemNowUs() : 0;
       if (aFrame && gFrame) {
         onNewIMUFrameSyncOutputCallback(aFrame, gFrame, arrival_system_us);
       } else if (log_imu_timestamps && (aFrame || gFrame)) {
-        imu_timestamp_csv_logger_->recordFrameSet(aFrame, gFrame, arrival_system_us, std::nullopt);
+        timestamp_csv_logger_->recordSyncedImu(aFrame, gFrame, arrival_system_us, std::nullopt);
       }
     });
 
@@ -6192,9 +6113,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()) {
-      (color_timestamp_csv_logger_ && color_timestamp_csv_logger_->enabled()) ||
-      (depth_timestamp_csv_logger_ && depth_timestamp_csv_logger_->enabled())) {
     const auto frame_set_arrival_system_us = getSystemNowUs();
     const auto frame_set_arrival_steady_us = getSteadyNowUs();
     auto final_color_frame = frame_set->getFrame(OB_FRAME_COLOR);
@@ -6204,23 +6123,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
     const bool color_publish_expected = track_color;
     const bool depth_publish_expected = track_depth;
 
-    if (frame_timestamp_csv_logger_) {
+    timestamp_csv_logger_->recordImageFrameSet(
-      frame_timestamp_csv_logger_->recordFrameSet(
+        final_color_frame, final_depth_frame, frame_set_arrival_system_us,
-          final_color_frame, final_depth_frame, frame_set_arrival_system_us,
+        frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected,
-          frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected,
+        depth_publish_expected);
-          depth_publish_expected);
-    } else {
-      if (track_color && color_timestamp_csv_logger_) {
-        color_timestamp_csv_logger_->recordStandaloneFrameArrival(
-            OB_STREAM_COLOR, final_color_frame, frame_set_arrival_system_us,
-            frame_set_arrival_steady_us, color_publish_expected);
-      }
-      if (track_depth && depth_timestamp_csv_logger_) {
-        depth_timestamp_csv_logger_->recordStandaloneFrameArrival(
-            OB_STREAM_DEPTH, final_depth_frame, frame_set_arrival_system_us,
-            frame_set_arrival_steady_us, depth_publish_expected);
-      }
-    }
   }
 
   try {
@@ -6714,17 +6620,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   if (frame == nullptr) {
     return;
   }
-  FrameTimestampCsvLogger *timestamp_csv_logger = nullptr;
+  const bool log_image_timestamps =
-  if (stream_index == COLOR) {
+      timestamp_csv_logger_ && timestamp_csv_logger_->imageStreamEnabled(stream_index.first);
-    timestamp_csv_logger = frame_timestamp_csv_logger_ ? frame_timestamp_csv_logger_.get()
-                                                       : color_timestamp_csv_logger_.get();
-  } else if (stream_index == DEPTH) {
-    timestamp_csv_logger = frame_timestamp_csv_logger_ ? frame_timestamp_csv_logger_.get()
-                                                       : depth_timestamp_csv_logger_.get();
-  }
   const auto record_image_publish_skipped = [&]() {
-    if (timestamp_csv_logger && timestamp_csv_logger->enabled()) {
+    if (log_image_timestamps) {
-      timestamp_csv_logger->recordImagePublishSkipped(stream_index.first, frame);
+      timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame);
     }
   };
   CHECK_NOTNULL(image_publishers_[stream_index]);
@@ -6841,10 +6741,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 && timestamp_csv_logger &&
+    if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) {
-        timestamp_csv_logger->enabled()) {
+      timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
-      timestamp_csv_logger->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(),
+                                                   getSteadyNowUs());
-                                                  getSteadyNowUs());
     }
     publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
     if (!has_raw_image_subscriber && stream_index == COLOR) {
@@ -6916,9 +6815,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       record_image_publish_skipped();
       return;
     }
-    if (timestamp_csv_logger && timestamp_csv_logger->enabled()) {
+    if (log_image_timestamps) {
-      timestamp_csv_logger->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(),
+      timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
-                                                  getSteadyNowUs());
+                                                   getSteadyNowUs());
     }
     if (stream_index == COLOR) {
       fps_delay_status_color_->tick(frame_timestamp);
@@ -7039,10 +6938,10 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
     return;
   }
   const auto record_timestamps = [&](std::optional<int64_t> publish_system_us) {
-    if (arrival_system_us != 0 && imu_timestamp_csv_logger_ &&
+    if (arrival_system_us != 0 && timestamp_csv_logger_ &&
-        imu_timestamp_csv_logger_->enabled()) {
+        timestamp_csv_logger_->syncedImuEnabled()) {
-      imu_timestamp_csv_logger_->recordFrameSet(accelframe, gryoframe, arrival_system_us,
+      timestamp_csv_logger_->recordSyncedImu(accelframe, gryoframe, arrival_system_us,
-                                                publish_system_us);
+                                             publish_system_us);
     }
   };
   if (!imu_gyro_accel_publisher_) {
@@ -7098,14 +6997,13 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
   if (!is_camera_node_initialized_.load() || !rclcpp::ok()) {
     return;
   }
-  auto *timestamp_csv_logger =
+  const bool log_imu_timestamps =
-      stream_index == ACCEL ? accel_timestamp_csv_logger_.get() : gyro_timestamp_csv_logger_.get();
+      timestamp_csv_logger_ && timestamp_csv_logger_->standaloneImuEnabled(stream_index.first);
-  const bool log_imu_timestamps = timestamp_csv_logger && timestamp_csv_logger->enabled();
   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->recordStandaloneFrame(stream_index.first, frame, arrival_system_us,
+      timestamp_csv_logger_->recordStandaloneImu(stream_index.first, frame, arrival_system_us,
-                                                  publish_system_us);
+                                                 publish_system_us);
     }
   };
   if (!imu_publishers_.count(stream_index)) {

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() || (!config.accel_enabled && !config.gyro_enabled)) {
+    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