refactor: centralize timestamp CSV logger management

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/imu_timestamp_csv_logger.h"
+#include "orbbec_camera/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<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::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