Merge branch 'feat/imu-timestamp-csv' into v2/develop

orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h

@@ -21,8 +21,10 @@ namespace orbbec_camera {
 
 class FrameTimestampCsvLogger {
  public:
+  enum class OutputMode { SYNCED, COLOR, DEPTH };
+
   FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
-                          rclcpp::Logger logger);
+                          OutputMode output_mode, rclcpp::Logger logger);
 
   ~FrameTimestampCsvLogger() noexcept;
 
@@ -137,7 +139,7 @@ class FrameTimestampCsvLogger {
   std::string serializeStreamColumns(const StreamState &state) const;
   static std::string formatSecondsColumn(int64_t time_us);
   static std::string formatOptionalIntColumn(const std::optional<int64_t> &value);
-  static std::string csvHeader();
+  std::string csvHeader() const;
 
   void writerThreadMain();
   std::string csvFilePathForIndex(uint64_t file_index) const;
@@ -152,6 +154,7 @@ class FrameTimestampCsvLogger {
   std::atomic_bool csv_writer_failed_{false};
   bool queue_warning_active_ = false;
   std::string csv_file_path_;
+  OutputMode output_mode_;
   std::ofstream csv_stream_;
   std::thread writer_thread_;
   uint64_t csv_file_index_ = 0;

orbbec_camera/include/orbbec_camera/imu_timestamp_csv_logger.h

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <atomic>
+#include <condition_variable>
+#include <cstdint>
+#include <deque>
+#include <fstream>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <thread>
+
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+
+class ImuTimestampCsvLogger {
+ public:
+  enum class OutputMode { SYNCED, ACCEL, GYRO };
+
+  ImuTimestampCsvLogger(const std::string &frame_csv_file_path, OutputMode output_mode,
+                        rclcpp::Logger logger);
+
+  ~ImuTimestampCsvLogger() noexcept;
+
+  ImuTimestampCsvLogger(const ImuTimestampCsvLogger &) = delete;
+  ImuTimestampCsvLogger &operator=(const ImuTimestampCsvLogger &) = delete;
+
+  void 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);
+
+  void recordStandaloneFrame(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                             int64_t arrival_system_us, std::optional<int64_t> publish_system_us);
+
+  void shutdown();
+
+  bool enabled() const { return enabled_; }
+
+ private:
+  struct StreamState {
+    bool has_frame = false;
+    int64_t device_ts_us = 0;
+    int64_t global_ts_us = 0;
+    int64_t sdk_system_ts_us = 0;
+    int64_t arrival_system_us = 0;
+    std::optional<int64_t> publish_system_us;
+  };
+
+  struct PendingRow {
+    uint64_t row_id = 0;
+    StreamState accel;
+    StreamState gyro;
+  };
+
+  void 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);
+  static void populateStreamState(StreamState &state, const std::shared_ptr<ob::Frame> &frame,
+                                  int64_t arrival_system_us,
+                                  std::optional<int64_t> publish_system_us);
+
+  void enqueueCompletedRow(const PendingRow &row);
+  std::string serializeRow(const PendingRow &row) const;
+  static std::string serializeStreamColumns(const StreamState &state);
+  static std::string formatSecondsColumn(int64_t time_us);
+  static std::string formatOptionalSecondsColumn(const std::optional<int64_t> &value);
+  std::string csvHeader() const;
+
+  void writerThreadMain();
+  std::string csvFilePathForIndex(uint64_t file_index) const;
+  bool openCsvFile(uint64_t file_index);
+  bool rotateCsvFile();
+
+  rclcpp::Logger logger_;
+  bool enabled_ = false;
+  bool csv_enabled_ = false;
+  std::atomic_bool shutdown_requested_{false};
+  std::atomic_bool csv_writer_failed_{false};
+  bool queue_warning_active_ = false;
+  std::string frame_csv_file_path_;
+  OutputMode output_mode_;
+  std::ofstream csv_stream_;
+  std::thread writer_thread_;
+  uint64_t csv_file_index_ = 0;
+  uint64_t csv_rows_written_ = 0;
+
+  uint64_t next_row_id_ = 1;
+  std::deque<PendingRow> completed_rows_;
+
+  std::mutex state_mutex_;
+  std::mutex completed_rows_mutex_;
+  std::condition_variable completed_rows_cv_;
+};
+
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -75,7 +75,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 "jpeg_decoder.h"
 #include <std_msgs/msg/header.hpp>
 #include <fcntl.h>
@@ -626,7 +626,8 @@ class OBCameraNode {
                        const std::shared_ptr<ob::Frame>& frame);
 
   void 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);
 
   void onNewIMUFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                              const stream_index_pair& stream_index);
@@ -1086,7 +1087,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::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