Merge branch 'feature/main-timestamp-stat' into merge/sdk_1.10.36

2026-09-13 03:30:18 +08:00 · 2026-06-18 18:25:53 +08:00
parent 2a8fd7759f 53a84a1433
commit e623642c58
36 changed files with 935 additions and 3 deletions
@@ -135,6 +135,7 @@ set(SOURCE_FILES
  src/d2c_viewer.cpp
  src/dynamic_params.cpp
  src/image_publisher.cpp
+  src/frame_timestamp_csv_logger.cpp
  src/ob_camera_node_driver.cpp
  src/ob_camera_node.cpp
  src/ros_param_backend.cpp
@@ -0,0 +1,166 @@
+#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;
+
+    std::optional<int64_t> metadata_frame_number;
+    std::optional<int64_t> sensor_ts_us;
+    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;
+    int64_t expected_interval_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> sensor_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> publish_sdk_system_ts_us;
+    int64_t expected_interval_us = 0;
+    int64_t dropped_frames = 0;
+    int64_t publish_dropped_frames = 0;
+    std::optional<int64_t> sensor_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);
+  void reportDropLogFormatOnce();
+
+  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;
+  bool drop_log_format_reported_ = 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
@@ -39,6 +39,7 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>

 #include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/compressed_image.hpp>
 #include <camera_info_manager/camera_info_manager.hpp>

 #include <image_publisher/image_publisher.hpp>
@@ -61,6 +62,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>

@@ -342,6 +344,10 @@ class OBCameraNode {
  void FillImuDataCopy(const IMUData& imu_data, std::deque<sensor_msgs::msg::Imu>& imu_msgs);

  bool setupFormatConvertType(OBFormat format);
+  bool hasCompressedImageSubscriber(const stream_index_pair& stream_index) const;
+  void publishCompressedColorImage(const std::shared_ptr<ob::Frame>& frame,
+                                   const stream_index_pair& stream_index,
+                                   const rclcpp::Time& timestamp, const std::string& frame_id);

  orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index);

@@ -401,6 +407,8 @@ class OBCameraNode {
  std::map<stream_index_pair, bool> flip_stream_;
  std::map<stream_index_pair, std::string> stream_name_;
  std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
+  std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr>
+      compressed_image_publishers_;
  std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
      camera_info_publishers_;

@@ -584,6 +592,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};
@@ -72,6 +72,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -80,6 +80,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -72,6 +72,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -72,6 +72,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
        DeclareLaunchArgument('diagnostic_period', default_value='0.0'),
    ]

@@ -72,6 +72,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -71,6 +71,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -72,6 +72,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -73,6 +73,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -56,6 +56,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -75,6 +75,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -125,6 +125,8 @@ def generate_launch_description():
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
        DeclareLaunchArgument('industry_mode', default_value=''),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),

        #color image transport plugins
        DeclareLaunchArgument('color.image_raw.enable_pub_plugins',default_value='["image_transport/compressed", "image_transport/raw", "image_transport/theora"]'),
@@ -55,6 +55,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -57,6 +57,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
        DeclareLaunchArgument('industry_mode', default_value=''),
    ]

@@ -57,6 +57,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -73,6 +73,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -55,6 +55,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -70,6 +70,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
        DeclareLaunchArgument('diagnostic_period', default_value='0.0'),
    ]

@@ -81,6 +81,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -89,6 +89,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='SW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -96,6 +96,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -99,6 +99,8 @@ def generate_launch_description():
        DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -96,6 +96,8 @@ def generate_launch_description():
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
        DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -88,6 +88,8 @@ def generate_launch_description():
        DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -104,6 +104,8 @@ def generate_launch_description():
        DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -207,6 +207,8 @@ def generate_launch_description():
        DeclareLaunchArgument('enable_3d_reconstruction_mode', default_value='false'),
        DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
        DeclareLaunchArgument('time_domain', default_value='device'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
        DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
        DeclareLaunchArgument('config_file_path', default_value=''),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
@@ -71,6 +71,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -56,6 +56,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -72,6 +72,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -58,6 +58,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),

    ]

@@ -73,6 +73,8 @@ def generate_launch_description():
        DeclareLaunchArgument('align_mode', default_value='HW'),
        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -66,6 +66,8 @@ def generate_launch_description():
        DeclareLaunchArgument('enable_soft_filter', default_value='true'),
        DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('enable_frame_timestamp_csv', default_value='false'),
+        DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''),
    ]

    # Node configuration
@@ -0,0 +1,595 @@
+#include "orbbec_camera/frame_timestamp_csv_logger.h"
+
+#include <algorithm>
+#include <chrono>
+#include <filesystem>
+#include <iomanip>
+#include <sstream>
+#include <utility>
+
+namespace orbbec_camera {
+namespace {
+
+constexpr size_t kCompletedQueueSoftLimit = 1000;
+constexpr size_t kFlushBatchSize = 100;
+constexpr auto kFlushInterval = std::chrono::seconds(1);
+
+int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
+  if (!frame || !frame->is<ob::VideoFrame>()) {
+    return 0;
+  }
+  auto stream_profile = frame->getStreamProfile();
+  if (!stream_profile || !stream_profile->is<ob::VideoStreamProfile>()) {
+    return 0;
+  }
+  const auto fps = stream_profile->as<ob::VideoStreamProfile>()->fps();
+  if (fps == 0) {
+    return 0;
+  }
+  return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
+}
+
+}  // namespace
+
+FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path,
+                                                 rclcpp::Logger logger)
+    : logger_(std::move(logger)), enabled_(enabled), csv_file_path_(csv_file_path) {
+  if (!enabled_) {
+    return;
+  }
+
+  if (csv_file_path_.empty()) {
+    RCLCPP_INFO_STREAM(logger_,
+                       "Frame timestamp CSV file is empty; only frame lost logs are enabled.");
+    return;
+  }
+
+  try {
+    auto path = std::filesystem::path(csv_file_path_);
+    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 frame timestamp CSV path " << csv_file_path_
+                                                                               << ": " << e.what());
+    enabled_ = false;
+    writer_failed_ = true;
+    return;
+  }
+
+  openCsvIfNeeded();
+  if (!enabled_) {
+    return;
+  }
+
+  writer_thread_ = std::thread([this]() { writerThreadMain(); });
+}
+
+FrameTimestampCsvLogger::~FrameTimestampCsvLogger() noexcept { shutdown(); }
+
+void FrameTimestampCsvLogger::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) {
+  if (!enabled_ || writer_failed_) {
+    return;
+  }
+  recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us,
+                         track_color, track_depth, color_image_publish_expected,
+                         depth_image_publish_expected);
+}
+
+void FrameTimestampCsvLogger::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) {
+  if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
+    return;
+  }
+  recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
+                                       image_publish_expected);
+}
+
+void FrameTimestampCsvLogger::recordPreImagePublish(OBStreamType stream_type,
+                                                    const std::shared_ptr<ob::Frame> &frame,
+                                                    int64_t publish_system_us,
+                                                    int64_t publish_steady_us) {
+  if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
+    return;
+  }
+  recordPreImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
+}
+
+void FrameTimestampCsvLogger::shutdown() {
+  if (!enabled_) {
+    return;
+  }
+
+  {
+    std::lock_guard<std::mutex> state_lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+    shutdown_requested_ = true;
+
+    std::vector<PendingRow> rows_to_flush;
+    flushPendingRowsLocked(rows_to_flush);
+    for (const auto &row : rows_to_flush) {
+      enqueueCompletedRow(row);
+    }
+  }
+
+  completed_rows_cv_.notify_all();
+  if (writer_thread_.joinable()) {
+    writer_thread_.join();
+  }
+
+  if (csv_stream_.is_open()) {
+    csv_stream_.flush();
+    csv_stream_.close();
+  }
+}
+
+FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream(
+    OBStreamType stream_type) const {
+  if (stream_type == OB_STREAM_COLOR) {
+    return TrackedStream::COLOR;
+  }
+  return TrackedStream::DEPTH;
+}
+
+bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const {
+  return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH;
+}
+
+void FrameTimestampCsvLogger::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) {
+  if (!track_color && !track_depth) {
+    return;
+  }
+
+  std::vector<PendingRow> ready_rows;
+  {
+    std::lock_guard<std::mutex> lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+
+    PendingRow row;
+    row.row_id = next_row_id_++;
+
+    if (track_color && color_frame) {
+      populateArrivalData(row.color, TrackedStream::COLOR, color_frame, arrival_system_us,
+                          arrival_steady_us, color_image_publish_expected);
+      color_frame_index_to_row_id_[row.color.frame_index] = row.row_id;
+      if (!color_image_publish_expected) {
+        finalizeStreamWithoutPublish(row.color);
+      }
+    } else {
+      row.color.final = true;
+    }
+
+    if (track_depth && depth_frame) {
+      populateArrivalData(row.depth, TrackedStream::DEPTH, depth_frame, arrival_system_us,
+                          arrival_steady_us, depth_image_publish_expected);
+      depth_frame_index_to_row_id_[row.depth.frame_index] = row.row_id;
+      if (!depth_image_publish_expected) {
+        finalizeStreamWithoutPublish(row.depth);
+      }
+    } else {
+      row.depth.final = true;
+    }
+
+    pending_rows_.emplace(row.row_id, row);
+    if (isRowReady(row)) {
+      auto it = pending_rows_.find(row.row_id);
+      if (it != pending_rows_.end()) {
+        ready_rows.push_back(it->second);
+        eraseFrameIndexMappingLocked(it->second);
+        pending_rows_.erase(it);
+      }
+    }
+  }
+
+  for (const auto &ready_row : ready_rows) {
+    enqueueCompletedRow(ready_row);
+  }
+}
+
+void FrameTimestampCsvLogger::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) {
+  std::optional<PendingRow> ready_row;
+  {
+    std::lock_guard<std::mutex> lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+
+    PendingRow row;
+    row.row_id = next_row_id_++;
+
+    const auto tracked_stream = toTrackedStream(stream_type);
+    auto &state = tracked_stream == TrackedStream::COLOR ? row.color : row.depth;
+    auto &other_state = tracked_stream == TrackedStream::COLOR ? row.depth : row.color;
+
+    populateArrivalData(state, tracked_stream, frame, arrival_system_us, arrival_steady_us,
+                        image_publish_expected);
+    other_state.final = true;
+
+    if (tracked_stream == TrackedStream::COLOR) {
+      color_frame_index_to_row_id_[state.frame_index] = row.row_id;
+    } else {
+      depth_frame_index_to_row_id_[state.frame_index] = row.row_id;
+    }
+
+    if (!image_publish_expected) {
+      finalizeStreamWithoutPublish(state);
+    }
+
+    pending_rows_.emplace(row.row_id, row);
+    if (isRowReady(row)) {
+      auto it = pending_rows_.find(row.row_id);
+      if (it != pending_rows_.end()) {
+        ready_row = it->second;
+        eraseFrameIndexMappingLocked(*ready_row);
+        pending_rows_.erase(it);
+      }
+    }
+  }
+
+  if (ready_row.has_value()) {
+    enqueueCompletedRow(*ready_row);
+  }
+}
+
+void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_type,
+                                                            const std::shared_ptr<ob::Frame> &frame,
+                                                            int64_t publish_system_us,
+                                                            int64_t publish_steady_us) {
+  std::optional<PendingRow> ready_row;
+  const auto frame_index = frame->index();
+
+  {
+    std::lock_guard<std::mutex> lock(state_mutex_);
+    if (shutdown_requested_) {
+      return;
+    }
+
+    const auto tracked_stream = toTrackedStream(stream_type);
+    auto &row_map = tracked_stream == TrackedStream::COLOR ? color_frame_index_to_row_id_
+                                                           : depth_frame_index_to_row_id_;
+    auto row_id_it = row_map.find(frame_index);
+    if (row_id_it == row_map.end()) {
+      RCLCPP_WARN_STREAM(logger_,
+                         "Frame timestamp CSV logger missed row mapping for stream "
+                             << (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
+                             << " frame index " << frame_index);
+      return;
+    }
+    const auto row_id = row_id_it->second;
+
+    auto pending_it = pending_rows_.find(row_id);
+    if (pending_it == pending_rows_.end()) {
+      return;
+    }
+
+    auto &state = tracked_stream == TrackedStream::COLOR ? pending_it->second.color
+                                                         : pending_it->second.depth;
+    populatePublishData(state, tracked_stream, publish_system_us, publish_steady_us);
+    state.final = true;
+
+    if (isRowReady(pending_it->second)) {
+      ready_row = pending_it->second;
+      eraseFrameIndexMappingLocked(*ready_row);
+      pending_rows_.erase(pending_it);
+    }
+  }
+
+  if (ready_row.has_value()) {
+    enqueueCompletedRow(*ready_row);
+  }
+}
+
+void FrameTimestampCsvLogger::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) {
+  auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_;
+
+  state.has_frame = true;
+  state.publish_expected = publish_expected;
+  state.frame_index = frame->index();
+  state.device_ts_us = static_cast<int64_t>(frame->timeStampUs());
+  state.global_ts_us = static_cast<int64_t>(frame->globalTimeStampUs());
+  state.sdk_system_ts_us = static_cast<int64_t>(frame->systemTimeStampUs());
+  if (previous.expected_interval_us <= 0) {
+    previous.expected_interval_us = getExpectedIntervalUs(frame);
+  }
+  state.expected_interval_us = previous.expected_interval_us;
+  if (previous.sdk_system_ts_us.has_value() && state.expected_interval_us > 0) {
+    const auto system_ts_delta_us = state.sdk_system_ts_us - previous.sdk_system_ts_us.value();
+    if (system_ts_delta_us > state.expected_interval_us * 3 / 2) {
+      const auto lost_frames =
+          std::max<int64_t>(1, system_ts_delta_us / state.expected_interval_us - 1);
+      previous.dropped_frames += lost_frames;
+      reportDropLogFormatOnce();
+      RCLCPP_WARN_STREAM(
+          logger_, "SDK drop " << (stream == TrackedStream::COLOR ? "color" : "depth") << ": idx="
+                               << state.frame_index << " ts=" << state.sdk_system_ts_us << "us"
+                               << " gap=" << std::fixed << std::setprecision(1)
+                               << (static_cast<double>(system_ts_delta_us) / 1000.0) << "ms"
+                               << " ideal="
+                               << (static_cast<double>(state.expected_interval_us) / 1000.0) << "ms"
+                               << " total=" << previous.dropped_frames);
+    }
+  }
+  if (frame->hasMetadata(OB_FRAME_METADATA_TYPE_FRAME_NUMBER)) {
+    state.metadata_frame_number =
+        static_cast<int64_t>(frame->getMetadataValue(OB_FRAME_METADATA_TYPE_FRAME_NUMBER));
+  } else {
+    state.metadata_frame_number.reset();
+  }
+  if (frame->hasMetadata(OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP)) {
+    state.sensor_ts_us =
+        static_cast<int64_t>(frame->getMetadataValue(OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP));
+  } else {
+    state.sensor_ts_us.reset();
+  }
+  state.arrival_system_us = arrival_system_us;
+  state.arrival_steady_us = arrival_steady_us;
+  state.device_ts_delta_us = updateDelta(previous.device_ts_us, state.device_ts_us);
+  if (state.sensor_ts_us.has_value()) {
+    state.sensor_ts_delta_us = updateDelta(previous.sensor_ts_us, state.sensor_ts_us.value());
+  } else {
+    state.sensor_ts_delta_us.reset();
+    previous.sensor_ts_us.reset();
+  }
+  state.global_ts_delta_us = updateDelta(previous.global_ts_us, state.global_ts_us);
+  state.sdk_system_ts_delta_us = updateDelta(previous.sdk_system_ts_us, state.sdk_system_ts_us);
+  previous.arrival_system_us = state.arrival_system_us;
+  state.arrival_steady_delta_us = updateDelta(previous.arrival_steady_us, state.arrival_steady_us);
+  state.sdk_delay_from_global_us = state.arrival_system_us - state.global_ts_us;
+  state.sdk_delay_from_system_us = state.arrival_system_us - state.sdk_system_ts_us;
+}
+
+void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStream stream,
+                                                  int64_t publish_system_us,
+                                                  int64_t publish_steady_us) {
+  auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_;
+
+  if (previous.publish_sdk_system_ts_us.has_value()) {
+    const auto system_ts_delta_us =
+        state.sdk_system_ts_us - previous.publish_sdk_system_ts_us.value();
+    if (state.expected_interval_us > 0 && system_ts_delta_us > state.expected_interval_us * 3 / 2) {
+      const auto lost_frames =
+          std::max<int64_t>(1, system_ts_delta_us / state.expected_interval_us - 1);
+      previous.publish_dropped_frames += lost_frames;
+      reportDropLogFormatOnce();
+      RCLCPP_WARN_STREAM(
+          logger_, "PUB drop " << (stream == TrackedStream::COLOR ? "color" : "depth") << ": idx="
+                               << state.frame_index << " ts=" << state.sdk_system_ts_us << "us"
+                               << " gap=" << std::fixed << std::setprecision(1)
+                               << (static_cast<double>(system_ts_delta_us) / 1000.0) << "ms"
+                               << " ideal="
+                               << (static_cast<double>(state.expected_interval_us) / 1000.0) << "ms"
+                               << " total=" << previous.publish_dropped_frames);
+    }
+  }
+  previous.publish_sdk_system_ts_us = state.sdk_system_ts_us;
+
+  state.publish_system_us = publish_system_us;
+  state.publish_steady_us = publish_steady_us;
+  previous.publish_system_us = state.publish_system_us.value();
+  state.publish_steady_delta_us =
+      updateDelta(previous.publish_steady_us, state.publish_steady_us.value());
+  state.arrival_to_publish_steady_us = state.publish_steady_us.value() - state.arrival_steady_us;
+}
+
+void FrameTimestampCsvLogger::reportDropLogFormatOnce() {
+  if (drop_log_format_reported_) {
+    return;
+  }
+  drop_log_format_reported_ = true;
+  RCLCPP_INFO_STREAM(
+      logger_,
+      "Frame drop log enabled. Format: <SDK|PUB> drop <color|depth>: idx=<frame_index> "
+      "ts=<system_timestamp_us> gap=<actual_interval_ms> ideal=<expected_interval_ms> "
+      "total=<total_dropped_frames>. SDK means frame arrival from SDK; PUB means before ROS "
+      "image publish.");
+}
+
+std::optional<int64_t> FrameTimestampCsvLogger::updateDelta(std::optional<int64_t> &previous,
+                                                            int64_t current) {
+  std::optional<int64_t> delta;
+  if (previous.has_value()) {
+    delta = current - previous.value();
+  }
+  previous = current;
+  return delta;
+}
+
+void FrameTimestampCsvLogger::finalizeStreamWithoutPublish(StreamState &state) {
+  state.final = true;
+}
+
+bool FrameTimestampCsvLogger::isRowReady(const PendingRow &row) const {
+  return row.color.final && row.depth.final;
+}
+
+void FrameTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) {
+  if (csv_file_path_.empty()) {
+    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_, "Frame timestamp CSV queue size exceeded "
+                                      << kCompletedQueueSoftLimit << " rows");
+      queue_warning_active_ = true;
+    }
+  } else {
+    queue_warning_active_ = false;
+  }
+  completed_rows_cv_.notify_one();
+}
+
+void FrameTimestampCsvLogger::flushPendingRowsLocked(std::vector<PendingRow> &rows) {
+  rows.reserve(rows.size() + pending_rows_.size());
+  for (auto &item : pending_rows_) {
+    auto row = item.second;
+    row.color.final = true;
+    row.depth.final = true;
+    rows.push_back(std::move(row));
+  }
+  pending_rows_.clear();
+  color_frame_index_to_row_id_.clear();
+  depth_frame_index_to_row_id_.clear();
+}
+
+void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row) {
+  if (row.color.has_frame) {
+    color_frame_index_to_row_id_.erase(row.color.frame_index);
+  }
+  if (row.depth.has_frame) {
+    depth_frame_index_to_row_id_.erase(row.depth.frame_index);
+  }
+}
+
+std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
+  std::ostringstream ss;
+  ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
+  return ss.str();
+}
+
+std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &state) const {
+  std::vector<std::string> fields(15, "");
+  if (state.has_frame) {
+    fields[0] = std::to_string(state.frame_index);
+    fields[1] = formatOptionalIntColumn(state.metadata_frame_number);
+    if (state.sensor_ts_us.has_value()) {
+      fields[2] = formatSecondsColumn(state.sensor_ts_us.value());
+    }
+    fields[3] = formatOptionalIntColumn(state.sensor_ts_delta_us);
+    fields[4] = formatSecondsColumn(state.device_ts_us);
+    fields[5] = formatOptionalIntColumn(state.device_ts_delta_us);
+    fields[6] = formatSecondsColumn(state.global_ts_us);
+    fields[7] = formatOptionalIntColumn(state.global_ts_delta_us);
+    fields[8] = formatSecondsColumn(state.sdk_system_ts_us);
+    fields[9] = formatOptionalIntColumn(state.sdk_system_ts_delta_us);
+    fields[10] = formatOptionalIntColumn(state.arrival_steady_delta_us);
+    fields[11] = formatOptionalIntColumn(state.publish_steady_delta_us);
+    fields[12] = formatOptionalIntColumn(state.arrival_to_publish_steady_us);
+    fields[13] = formatOptionalIntColumn(state.sdk_delay_from_global_us);
+    fields[14] = formatOptionalIntColumn(state.sdk_delay_from_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 FrameTimestampCsvLogger::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 FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional<int64_t> &value) {
+  if (!value.has_value()) {
+    return "";
+  }
+  return std::to_string(*value);
+}
+
+std::string FrameTimestampCsvLogger::csvHeader() {
+  std::ostringstream ss;
+  for (const auto *prefix : {"color", "depth"}) {
+    ss << prefix << "_sdk_frame_index,";
+    ss << prefix << "_hardware_frame_number,";
+    ss << prefix << "_sensor_ts_sec,";
+    ss << prefix << "_sensor_ts_delta_us,";
+    ss << prefix << "_device_ts_sec,";
+    ss << prefix << "_device_ts_delta_us,";
+    ss << prefix << "_global_ts_sec,";
+    ss << prefix << "_global_ts_delta_us,";
+    ss << prefix << "_system_ts_sec,";
+    ss << prefix << "_system_ts_delta_us,";
+    ss << prefix << "_arrival_steady_delta_us,";
+    ss << prefix << "_publish_steady_delta_us,";
+    ss << prefix << "_arrival_to_publish_steady_us,";
+    ss << prefix << "_sdk_delay_from_global_us,";
+    ss << prefix << "_sdk_delay_from_system_us";
+    if (std::string(prefix) == "color") {
+      ss << ",";
+    }
+  }
+  return ss.str();
+}
+
+void FrameTimestampCsvLogger::writerThreadMain() {
+  if (!enabled_ || 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_);
+    }
+
+    for (const auto &row : rows_to_write) {
+      if (csv_stream_.is_open()) {
+        csv_stream_ << serializeRow(row) << "\n";
+        rows_since_flush++;
+      }
+    }
+
+    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;
+    }
+  }
+}
+
+void FrameTimestampCsvLogger::openCsvIfNeeded() {
+  csv_stream_.open(csv_file_path_, std::ios::out | std::ios::trunc);
+  if (!csv_stream_.is_open()) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to open frame timestamp CSV file: " << csv_file_path_);
+    enabled_ = false;
+    writer_failed_ = true;
+    return;
+  }
+  csv_stream_ << csvHeader() << "\n";
+  csv_stream_.flush();
+  RCLCPP_INFO_STREAM(logger_, "Frame timestamp CSV logger enabled: " << csv_file_path_);
+}
+
+}  // namespace orbbec_camera
@@ -34,6 +34,22 @@
 namespace orbbec_camera {
 using namespace std::chrono_literals;

+namespace {
+
+int64_t getSystemNowUs() {
+  return std::chrono::duration_cast<std::chrono::microseconds>(
+             std::chrono::system_clock::now().time_since_epoch())
+      .count();
+}
+
+int64_t getSteadyNowUs() {
+  return std::chrono::duration_cast<std::chrono::microseconds>(
+             std::chrono::steady_clock::now().time_since_epoch())
+      .count();
+}
+
+}  // namespace
+
 OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
                           std::shared_ptr<Parameters> parameters, bool use_intra_process)
    : node_(node),
@@ -58,6 +74,13 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
  compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
  setupDefaultImageFormat();
  setupTopics();
+  if (enable_frame_timestamp_csv_) {
+    frame_timestamp_csv_logger_ =
+        std::make_unique<FrameTimestampCsvLogger>(true, frame_timestamp_csv_file_, logger_);
+    if (!frame_timestamp_csv_logger_->enabled()) {
+      frame_timestamp_csv_logger_.reset();
+    }
+  }
 #if defined(USE_RK_HW_DECODER)
  jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
 #elif defined(USE_NV_HW_DECODER)
@@ -110,6 +133,10 @@ void OBCameraNode::clean() noexcept {
  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
  RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
  is_running_.store(false);
+  if (frame_timestamp_csv_logger_) {
+    frame_timestamp_csv_logger_->shutdown();
+    frame_timestamp_csv_logger_.reset();
+  }
  RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
  if (tf_thread_ && tf_thread_->joinable()) {
    tf_thread_->join();
@@ -1247,6 +1274,8 @@ void OBCameraNode::getParameters() {
  setAndGetNodeParameter<int>(depth_ae_roi_bottom_, "depth_ae_roi_bottom", -1);

  setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "device");
+  setAndGetNodeParameter<bool>(enable_frame_timestamp_csv_, "enable_frame_timestamp_csv", false);
+  setAndGetNodeParameter<std::string>(frame_timestamp_csv_file_, "frame_timestamp_csv_file", "");
  auto device_info = device_->getDeviceInfo();
  CHECK_NOTNULL(device_info.get());
  auto pid = device_info->pid();
@@ -1407,13 +1436,22 @@ void OBCameraNode::setupPublishers() {
    if (use_intra_process_) {
      image_qos_profile = rmw_qos_profile_default;
    }
-    if (use_intra_process_) {
+    const bool is_mjpg_color_stream =
+        stream_index == COLOR && format_[stream_index] == OB_FORMAT_MJPG;
+    if (use_intra_process_ || is_mjpg_color_stream) {
      image_publishers_[stream_index] =
          std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
    } else {
      image_publishers_[stream_index] =
          std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
    }
+    if (is_mjpg_color_stream) {
+      compressed_image_publishers_[stream_index] =
+          node_->create_publisher<sensor_msgs::msg::CompressedImage>(
+              topic + "/compressed",
+              rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile),
+                          image_qos_profile));
+    }

    topic = name + "/camera_info";
    auto camera_info_qos = camera_info_qos_[stream_index];
@@ -1820,6 +1858,22 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
  if (frame_set == nullptr) {
    return;
  }
+  if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled()) {
+    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);
+    auto final_depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
+    const bool track_color = enable_stream_[COLOR] && static_cast<bool>(final_color_frame);
+    const bool track_depth = enable_stream_[DEPTH] && static_cast<bool>(final_depth_frame);
+    const bool color_publish_expected = track_color;
+    const bool depth_publish_expected = track_depth;
+
+    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);
+  }
+
  try {
    if (!tf_published_) {
      publishStaticTransforms();
@@ -2031,8 +2085,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
  if (frame == nullptr) {
    return;
  }
-  CHECK_NOTNULL(image_publishers_[stream_index]);
-  bool has_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0;
+  CHECK_NOTNULL(image_publishers_.at(stream_index));
+  const bool has_raw_image_subscriber =
+      image_publishers_.at(stream_index)->get_subscription_count() > 0;
+  const bool has_compressed_image_subscriber = hasCompressedImageSubscriber(stream_index);
+  bool has_subscriber = has_raw_image_subscriber || has_compressed_image_subscriber;
  has_subscriber =
      has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0;
  has_subscriber =
@@ -2165,6 +2222,18 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
  if (isGemini335PID(pid)) {
    publishMetadata(frame, stream_index, camera_info.header);
  }
+  if (stream_index == COLOR && frame->format() == OB_FORMAT_MJPG &&
+      has_compressed_image_subscriber) {
+    publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
+    // Record CSV publish timestamp here only when there is no raw image subscriber.
+    // If a raw subscriber also exists, the existing logging at the raw publish site will record
+    // it, avoiding a double call that would corrupt publish_steady_delta_us.
+    if (!has_raw_image_subscriber && frame_timestamp_csv_logger_ &&
+        frame_timestamp_csv_logger_->enabled()) {
+      frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
+                                                         getSystemNowUs(), getSteadyNowUs());
+    }
+  }
  CHECK_NOTNULL(image_publishers_[stream_index]);
  if (image_publishers_[stream_index]->get_subscription_count() == 0) {
    return;
@@ -2201,6 +2270,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
  image_msg->header.frame_id = frame_id;
  CHECK(image_publishers_.count(stream_index) > 0);
  saveImageToFile(stream_index, image, *image_msg);
+  if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
+      (stream_index == COLOR || stream_index == DEPTH)) {
+    frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(),
+                                                       getSteadyNowUs());
+  }
  image_publishers_[stream_index]->publish(std::move(image_msg));
  if (stream_index == COLOR && enable_color_undistortion_ &&
      color_undistortion_publisher_->get_subscription_count() > 0) {
@@ -2817,4 +2891,27 @@ orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo(
  return imu_info;
 }

+bool OBCameraNode::hasCompressedImageSubscriber(const stream_index_pair &stream_index) const {
+  auto it = compressed_image_publishers_.find(stream_index);
+  return it != compressed_image_publishers_.end() && it->second &&
+         it->second->get_subscription_count() > 0;
+}
+
+void OBCameraNode::publishCompressedColorImage(const std::shared_ptr<ob::Frame> &frame,
+                                               const stream_index_pair &stream_index,
+                                               const rclcpp::Time &timestamp,
+                                               const std::string &frame_id) {
+  auto it = compressed_image_publishers_.find(stream_index);
+  if (it == compressed_image_publishers_.end() || !it->second) {
+    return;
+  }
+  sensor_msgs::msg::CompressedImage msg;
+  msg.header.stamp = timestamp;
+  msg.header.frame_id = frame_id;
+  msg.format = "jpeg";
+  const auto *data = static_cast<const uint8_t *>(frame->data());
+  msg.data.assign(data, data + frame->dataSize());
+  it->second->publish(std::move(msg));
+}
+
 }  // namespace orbbec_camera