refactor: centralize timestamp CSV logger management

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