feat: log IMU frame timestamps to CSV

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

@@ -742,6 +742,31 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
     if (!frame_timestamp_csv_logger_->enabled()) {
       frame_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();
+          }
+        }
+      }
+    }
   }
 
   if (enable_d2c_viewer_) {
@@ -818,6 +843,30 @@ void OBCameraNode::clean() noexcept {
   } catch (...) {
     RCLCPP_WARN_STREAM(logger_, "Exception while shutting down frame 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
   try {
@@ -4128,8 +4177,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() &&
+                                      imu_timestamp_csv_logger_ &&
+                                      imu_timestamp_csv_logger_->enabled();
+      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)) {
+        imu_timestamp_csv_logger_->recordFrameSet(aFrame, gFrame, arrival_system_us, std::nullopt);
       }
     });
 
@@ -6921,11 +6976,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 && imu_timestamp_csv_logger_ &&
+        imu_timestamp_csv_logger_->enabled()) {
+      imu_timestamp_csv_logger_->recordFrameSet(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;
   }
@@ -6933,6 +6997,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();
@@ -6966,7 +7031,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,
@@ -6974,7 +7041,18 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &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 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,
+                                                  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;
@@ -6983,6 +7061,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();
@@ -7009,10 +7088,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) {