From 09990b6bf1ef4e4a58cbdfb86fc7b1e5c3075c20 Mon Sep 17 00:00:00 2001 From: slz Date: Tue, 25 Aug 2026 16:11:38 +0800 Subject: [PATCH] refactor: centralize timestamp CSV logger management --- orbbec_camera/CMakeLists.txt | 1 + .../include/orbbec_camera/ob_camera_node.h | 10 +- .../orbbec_camera/timestamp_csv_logger.h | 73 +++++++ orbbec_camera/src/ob_camera_node.cpp | 186 ++++------------- orbbec_camera/src/timestamp_csv_logger.cpp | 194 ++++++++++++++++++ 5 files changed, 312 insertions(+), 152 deletions(-) create mode 100644 orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h create mode 100644 orbbec_camera/src/timestamp_csv_logger.cpp diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index b5db9ba5..af342a47 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/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 diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index ca9db1b4..26dce805 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/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 #include @@ -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 frame_timestamp_csv_logger_; - std::unique_ptr color_timestamp_csv_logger_; - std::unique_ptr depth_timestamp_csv_logger_; - std::unique_ptr imu_timestamp_csv_logger_; - std::unique_ptr accel_timestamp_csv_logger_; - std::unique_ptr gyro_timestamp_csv_logger_; + std::unique_ptr timestamp_csv_logger_; std::string exposure_range_mode_; std::string load_config_json_file_path_ = ""; std::string export_config_json_file_path_ = ""; diff --git a/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h new file mode 100644 index 00000000..8c3029de --- /dev/null +++ b/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h @@ -0,0 +1,73 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#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 &color_frame, + const std::shared_ptr &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 &frame, + int64_t publish_system_us, int64_t publish_steady_us); + void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr &frame); + + void recordSyncedImu(const std::shared_ptr &accel_frame, + const std::shared_ptr &gyro_frame, int64_t arrival_system_us, + std::optional publish_system_us); + void recordStandaloneImu(OBStreamType stream_type, const std::shared_ptr &frame, + int64_t arrival_system_us, std::optional 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 synced_image_logger_; + std::unique_ptr color_logger_; + std::unique_ptr depth_logger_; + std::unique_ptr synced_imu_logger_; + std::unique_ptr accel_logger_; + std::unique_ptr gyro_logger_; +}; + +} // namespace orbbec_camera diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 72876b66..b27ce7e6 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -738,54 +738,19 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic setupTopics(); if (enable_frame_drop_log_ || !frame_timestamp_csv_file_.empty()) { - if (enable_frame_sync_ && (enable_stream_[COLOR] || enable_stream_[DEPTH])) { - frame_timestamp_csv_logger_ = std::make_unique( - enable_frame_drop_log_, frame_timestamp_csv_file_, - FrameTimestampCsvLogger::OutputMode::SYNCED, logger_); - if (!frame_timestamp_csv_logger_->enabled()) { - frame_timestamp_csv_logger_.reset(); - } - } else { - if (enable_stream_[COLOR]) { - color_timestamp_csv_logger_ = std::make_unique( - enable_frame_drop_log_, frame_timestamp_csv_file_, - FrameTimestampCsvLogger::OutputMode::COLOR, logger_); - if (!color_timestamp_csv_logger_->enabled()) { - color_timestamp_csv_logger_.reset(); - } - } - if (enable_stream_[DEPTH]) { - depth_timestamp_csv_logger_ = std::make_unique( - 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( - 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( - 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( - frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::GYRO, logger_); - if (!gyro_timestamp_csv_logger_->enabled()) { - gyro_timestamp_csv_logger_.reset(); - } - } - } + TimestampCsvLogger::Config timestamp_config; + timestamp_config.frame_drop_log_enabled = enable_frame_drop_log_; + timestamp_config.csv_file_path = frame_timestamp_csv_file_; + 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(std::move(timestamp_config), logger_); + if (!timestamp_csv_logger_->enabled()) { + timestamp_csv_logger_.reset(); } } @@ -855,53 +820,9 @@ 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"); - } - 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"); + if (timestamp_csv_logger_) { + timestamp_csv_logger_->shutdown(); + timestamp_csv_logger_.reset(); } // 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_ && - imu_timestamp_csv_logger_->enabled(); + timestamp_csv_logger_ && + 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 frame_set if (frame_set == nullptr) { return; } - if ((frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) || - (color_timestamp_csv_logger_ && color_timestamp_csv_logger_->enabled()) || - (depth_timestamp_csv_logger_ && depth_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); @@ -6204,23 +6123,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set const bool color_publish_expected = track_color; const bool depth_publish_expected = track_depth; - if (frame_timestamp_csv_logger_) { - 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); - } 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); - } - } + 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); } try { @@ -6714,17 +6620,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, if (frame == nullptr) { return; } - FrameTimestampCsvLogger *timestamp_csv_logger = nullptr; - if (stream_index == COLOR) { - 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 bool log_image_timestamps = + timestamp_csv_logger_ && timestamp_csv_logger_->imageStreamEnabled(stream_index.first); const auto record_image_publish_skipped = [&]() { - if (timestamp_csv_logger && timestamp_csv_logger->enabled()) { - timestamp_csv_logger->recordImagePublishSkipped(stream_index.first, frame); + if (log_image_timestamps) { + 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 &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 && - timestamp_csv_logger->enabled()) { - timestamp_csv_logger->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(), - getSteadyNowUs()); + if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) { + timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(), + 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 &frame, record_image_publish_skipped(); return; } - if (timestamp_csv_logger && timestamp_csv_logger->enabled()) { - timestamp_csv_logger->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(), - getSteadyNowUs()); + if (log_image_timestamps) { + timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(), + getSteadyNowUs()); } if (stream_index == COLOR) { fps_delay_status_color_->tick(frame_timestamp); @@ -7039,10 +6938,10 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr publish_system_us) { - if (arrival_system_us != 0 && imu_timestamp_csv_logger_ && - imu_timestamp_csv_logger_->enabled()) { - imu_timestamp_csv_logger_->recordFrameSet(accelframe, gryoframe, arrival_system_us, - 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_) { @@ -7098,14 +6997,13 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr &frame if (!is_camera_node_initialized_.load() || !rclcpp::ok()) { return; } - auto *timestamp_csv_logger = - stream_index == ACCEL ? accel_timestamp_csv_logger_.get() : gyro_timestamp_csv_logger_.get(); - const bool log_imu_timestamps = timestamp_csv_logger && timestamp_csv_logger->enabled(); + 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 publish_system_us) { if (log_imu_timestamps) { - timestamp_csv_logger->recordStandaloneFrame(stream_index.first, frame, arrival_system_us, - publish_system_us); + timestamp_csv_logger_->recordStandaloneImu(stream_index.first, frame, arrival_system_us, + publish_system_us); } }; if (!imu_publishers_.count(stream_index)) { diff --git a/orbbec_camera/src/timestamp_csv_logger.cpp b/orbbec_camera/src/timestamp_csv_logger.cpp new file mode 100644 index 00000000..291f3d9d --- /dev/null +++ b/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 +#include + +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( + 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(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 &color_frame, + const std::shared_ptr &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 &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 &frame) { + auto *timestamp_logger = imageLoggerForStream(stream_type); + if (timestamp_logger) { + timestamp_logger->recordImagePublishSkipped(stream_type, frame); + } +} + +void TimestampCsvLogger::recordSyncedImu(const std::shared_ptr &accel_frame, + const std::shared_ptr &gyro_frame, + int64_t arrival_system_us, + std::optional 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 &frame, + int64_t arrival_system_us, + std::optional 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 ×tamp_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