feat: add frame timestamp CSV logging functionality

orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h

@@ -0,0 +1,155 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <atomic>
+#include <condition_variable>
+#include <cstdint>
+#include <deque>
+#include <fstream>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <thread>
+#include <unordered_map>
+#include <vector>
+
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+
+class FrameTimestampCsvLogger {
+ public:
+  FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path, rclcpp::Logger logger);
+
+  ~FrameTimestampCsvLogger() noexcept;
+
+  FrameTimestampCsvLogger(const FrameTimestampCsvLogger &) = delete;
+  FrameTimestampCsvLogger &operator=(const FrameTimestampCsvLogger &) = delete;
+
+  void recordFrameSet(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 recordStandaloneFrameArrival(OBStreamType stream_type,
+                                    const std::shared_ptr<ob::Frame> &frame,
+                                    int64_t arrival_system_us, int64_t arrival_steady_us,
+                                    bool image_publish_expected);
+
+  void recordPreImagePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                             int64_t publish_system_us, int64_t publish_steady_us);
+
+  void shutdown();
+
+  bool enabled() const { return enabled_; }
+
+ private:
+  enum class TrackedStream { COLOR, DEPTH };
+
+  struct StreamState {
+    bool has_frame = false;
+    bool publish_expected = false;
+    bool final = false;
+    uint64_t frame_index = 0;
+
+    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;
+    int64_t arrival_steady_us = 0;
+    std::optional<int64_t> publish_system_us;
+    std::optional<int64_t> publish_steady_us;
+
+    std::optional<int64_t> device_ts_delta_us;
+    std::optional<int64_t> global_ts_delta_us;
+    std::optional<int64_t> sdk_system_ts_delta_us;
+    std::optional<int64_t> arrival_system_delta_us;
+    std::optional<int64_t> arrival_steady_delta_us;
+    std::optional<int64_t> publish_system_delta_us;
+    std::optional<int64_t> publish_steady_delta_us;
+    std::optional<int64_t> arrival_to_publish_system_us;
+    std::optional<int64_t> arrival_to_publish_steady_us;
+    std::optional<int64_t> sdk_delay_from_global_us;
+    std::optional<int64_t> sdk_delay_from_system_us;
+  };
+
+  struct PendingRow {
+    uint64_t row_id = 0;
+    StreamState color;
+    StreamState depth;
+  };
+
+  struct PreviousStreamTimestamps {
+    std::optional<int64_t> device_ts_us;
+    std::optional<int64_t> global_ts_us;
+    std::optional<int64_t> sdk_system_ts_us;
+    std::optional<int64_t> arrival_system_us;
+    std::optional<int64_t> arrival_steady_us;
+    std::optional<int64_t> publish_system_us;
+    std::optional<int64_t> publish_steady_us;
+  };
+
+  TrackedStream toTrackedStream(OBStreamType stream_type) const;
+  bool isTrackedStream(OBStreamType stream_type) const;
+
+  void recordFrameSetInternal(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 recordStandaloneFrameArrivalInternal(OBStreamType stream_type,
+                                            const std::shared_ptr<ob::Frame> &frame,
+                                            int64_t arrival_system_us, int64_t arrival_steady_us,
+                                            bool image_publish_expected);
+  void recordPreImagePublishInternal(OBStreamType stream_type,
+                                     const std::shared_ptr<ob::Frame> &frame,
+                                     int64_t publish_system_us, int64_t publish_steady_us);
+
+  void populateArrivalData(StreamState &state, TrackedStream stream,
+                           const std::shared_ptr<ob::Frame> &frame, int64_t arrival_system_us,
+                           int64_t arrival_steady_us, bool publish_expected);
+  void populatePublishData(StreamState &state, TrackedStream stream, int64_t publish_system_us,
+                           int64_t publish_steady_us);
+
+  std::optional<int64_t> updateDelta(std::optional<int64_t> &previous, int64_t current);
+
+  void finalizeStreamWithoutPublish(StreamState &state);
+  bool isRowReady(const PendingRow &row) const;
+  void enqueueCompletedRow(const PendingRow &row);
+  void flushPendingRowsLocked(std::vector<PendingRow> &rows);
+  void eraseFrameIndexMappingLocked(const PendingRow &row);
+
+  std::string serializeRow(const PendingRow &row) const;
+  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();
+
+  void writerThreadMain();
+  void openCsvIfNeeded();
+
+  rclcpp::Logger logger_;
+  bool enabled_ = false;
+  std::atomic_bool shutdown_requested_{false};
+  bool writer_failed_ = false;
+  bool queue_warning_active_ = false;
+  std::string csv_file_path_;
+  std::ofstream csv_stream_;
+  std::thread writer_thread_;
+
+  uint64_t next_row_id_ = 1;
+  std::unordered_map<uint64_t, PendingRow> pending_rows_;
+  std::unordered_map<uint64_t, uint64_t> color_frame_index_to_row_id_;
+  std::unordered_map<uint64_t, uint64_t> depth_frame_index_to_row_id_;
+  PreviousStreamTimestamps color_previous_;
+  PreviousStreamTimestamps depth_previous_;
+
+  std::deque<PendingRow> completed_rows_;
+
+  mutable 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

@@ -61,6 +61,7 @@
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
 #include "orbbec_camera/image_publisher.h"
+#include "orbbec_camera/frame_timestamp_csv_logger.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/string.hpp>
 
@@ -584,6 +585,9 @@ class OBCameraNode {
   int min_depth_limit_ = 0;
   int max_depth_limit_ = 0;
   std::string time_domain_ = "device";  // device, system, global
+  bool enable_frame_timestamp_csv_ = false;
+  std::string frame_timestamp_csv_file_;
+  std::unique_ptr<FrameTimestampCsvLogger> frame_timestamp_csv_logger_;
   // soft ware trigger
   rclcpp::TimerBase::SharedPtr software_trigger_timer_;
   std::chrono::milliseconds software_trigger_period_{33};