feat: split image timestamp CSV files when frame sync is disabled

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/ob_camera_node.h

@@ -1024,6 +1024,8 @@ class OBCameraNode {
   bool enable_frame_drop_log_ = false;
   std::string frame_timestamp_csv_file_;
   std::unique_ptr<FrameTimestampCsvLogger> frame_timestamp_csv_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> color_timestamp_csv_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> depth_timestamp_csv_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> imu_timestamp_csv_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> accel_timestamp_csv_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> gyro_timestamp_csv_logger_;

orbbec_camera/src/frame_timestamp_csv_logger.cpp

@@ -35,25 +35,26 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
 
 FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
                                                  const std::string &csv_file_path,
-                                                 rclcpp::Logger logger)
+                                                 OutputMode output_mode, rclcpp::Logger logger)
     : logger_(std::move(logger)),
       enabled_(drop_log_enabled || !csv_file_path.empty()),
       csv_enabled_(!csv_file_path.empty()),
       drop_log_enabled_(drop_log_enabled),
-      csv_file_path_(csv_file_path) {
+      csv_file_path_(csv_file_path),
+      output_mode_(output_mode) {
   if (!enabled_) {
     return;
   }
 
   if (csv_enabled_) {
     try {
-      auto path = std::filesystem::path(csv_file_path_);
+      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 frame timestamp CSV path "
-                                       << csv_file_path_ << ": " << e.what());
+                                       << csvFilePathForIndex(0) << ": " << e.what());
       csv_enabled_ = false;
       csv_writer_failed_ = true;
     }
@@ -74,7 +75,7 @@ FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
   if (enabled_) {
     RCLCPP_INFO_STREAM(logger_,
                        "Frame timestamp logger enabled: csv_file="
-                           << (csv_enabled_ ? csv_file_path_ : "disabled")
+                           << (csv_enabled_ ? csvFilePathForIndex(0) : "disabled")
                            << " frame_drop_log=" << (drop_log_enabled_ ? "enabled" : "disabled"));
   }
 }
@@ -90,6 +91,9 @@ void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &c
   if (!enabled_) {
     return;
   }
+  if (output_mode_ != OutputMode::SYNCED) {
+    return;
+  }
   recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us,
                          track_color, track_depth, color_image_publish_expected,
                          depth_image_publish_expected);
@@ -100,7 +104,9 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t
                                                            int64_t arrival_system_us,
                                                            int64_t arrival_steady_us,
                                                            bool image_publish_expected) {
-  if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
+  if (!enabled_ || !frame || !isTrackedStream(stream_type) ||
+      (stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) ||
+      (stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) {
     return;
   }
   recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -479,6 +485,12 @@ void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row
 }
 
 std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
+  if (output_mode_ == OutputMode::COLOR) {
+    return serializeStreamColumns(row.color);
+  }
+  if (output_mode_ == OutputMode::DEPTH) {
+    return serializeStreamColumns(row.depth);
+  }
   std::ostringstream ss;
   ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
   return ss.str();
@@ -529,9 +541,9 @@ std::string FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional
   return std::to_string(*value);
 }
 
-std::string FrameTimestampCsvLogger::csvHeader() {
+std::string FrameTimestampCsvLogger::csvHeader() const {
   std::ostringstream ss;
-  for (const auto *prefix : {"color", "depth"}) {
+  const auto append_stream_header = [&ss](const char *prefix) {
     ss << prefix << "_sdk_frame_index,";
     ss << prefix << "_hardware_frame_number,";
     ss << prefix << "_sensor_ts_sec,";
@@ -547,9 +559,15 @@ std::string FrameTimestampCsvLogger::csvHeader() {
     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") {
+  };
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) {
+    append_stream_header("color");
+  }
+  if (output_mode_ == OutputMode::SYNCED) {
     ss << ",";
   }
+  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
+    append_stream_header("depth");
   }
   return ss.str();
 }
@@ -621,13 +639,19 @@ void FrameTimestampCsvLogger::writerThreadMain() {
 }
 
 std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
-  if (file_index == 0) {
+  const std::filesystem::path original_path(csv_file_path_);
-    return csv_file_path_;
+  std::string suffix;
+  if (output_mode_ == OutputMode::COLOR) {
+    suffix = "_color";
+  } else if (output_mode_ == OutputMode::DEPTH) {
+    suffix = "_depth";
   }
 
-  const std::filesystem::path original_path(csv_file_path_);
+  auto indexed_filename = original_path.stem().string() + suffix;
-  const auto indexed_filename = original_path.stem().string() + "_" + std::to_string(file_index) +
+  if (file_index != 0) {
-                                original_path.extension().string();
+    indexed_filename += "_" + std::to_string(file_index);
+  }
+  indexed_filename += original_path.extension().string();
   return (original_path.parent_path() / indexed_filename).string();
 }
 

orbbec_camera/src/ob_camera_node.cpp

@@ -452,7 +452,8 @@ void OBCameraNode::publishDepthFiltersStatus() {
     depth_filters_snapshot = depth_filter_list_;
   }
 
-  auto find_depth_filter = [&depth_filters_snapshot](const std::string &filter_name) -> std::shared_ptr<ob::Filter> {
+  auto find_depth_filter =
+      [&depth_filters_snapshot](const std::string &filter_name) -> std::shared_ptr<ob::Filter> {
     const auto normalized_name = normalizeDepthFilterName(filter_name);
     auto it = std::find_if(depth_filters_snapshot.begin(), depth_filters_snapshot.end(),
                            [&normalized_name](const auto &filter) {
@@ -737,13 +738,32 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> 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<FrameTimestampCsvLogger>(
-        enable_frame_drop_log_, frame_timestamp_csv_file_, logger_);
+          enable_frame_drop_log_, frame_timestamp_csv_file_,
+          FrameTimestampCsvLogger::OutputMode::SYNCED, logger_);
       if (!frame_timestamp_csv_logger_->enabled()) {
         frame_timestamp_csv_logger_.reset();
       }
-    if (!frame_timestamp_csv_file_.empty() &&
+    } else {
-        (enable_stream_[ACCEL] || enable_stream_[GYRO])) {
+      if (enable_stream_[COLOR]) {
+        color_timestamp_csv_logger_ = std::make_unique<FrameTimestampCsvLogger>(
+            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<FrameTimestampCsvLogger>(
+            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<ImuTimestampCsvLogger>(
             frame_timestamp_csv_file_, ImuTimestampCsvLogger::OutputMode::SYNCED, logger_);
@@ -843,6 +863,22 @@ void OBCameraNode::clean() noexcept {
   } 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();
@@ -6156,7 +6192,9 @@ 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()) {
+  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())) {
     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);
@@ -6166,10 +6204,23 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> 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);
+      }
+    }
   }
 
   try {
@@ -6611,7 +6662,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
       target_buffer_size = &rgb_buffer_size_;
     }
     if (video_frame->getDataSize() > *target_buffer_size) {
-      delete[](*target_buffer);
+      delete[] (*target_buffer);
       *target_buffer_size = video_frame->getDataSize();
       *target_buffer = new uint8_t[*target_buffer_size];
       buffer = *target_buffer;
@@ -6663,10 +6714,17 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &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 auto record_image_publish_skipped = [&]() {
-    if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
+    if (timestamp_csv_logger && timestamp_csv_logger->enabled()) {
-        (stream_index == COLOR || stream_index == DEPTH)) {
+      timestamp_csv_logger->recordImagePublishSkipped(stream_index.first, frame);
-      frame_timestamp_csv_logger_->recordImagePublishSkipped(stream_index.first, frame);
     }
   };
   CHECK_NOTNULL(image_publishers_[stream_index]);
@@ -6783,10 +6841,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &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 && frame_timestamp_csv_logger_ &&
+    if (!has_raw_image_subscriber && stream_index == COLOR && timestamp_csv_logger &&
-        frame_timestamp_csv_logger_->enabled()) {
+        timestamp_csv_logger->enabled()) {
-      frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
+      timestamp_csv_logger->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(),
-                                                         getSystemNowUs(), getSteadyNowUs());
+                                                  getSteadyNowUs());
     }
     publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
     if (!has_raw_image_subscriber && stream_index == COLOR) {
@@ -6858,10 +6916,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       record_image_publish_skipped();
       return;
     }
-    if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
+    if (timestamp_csv_logger && timestamp_csv_logger->enabled()) {
-        (stream_index == COLOR || stream_index == DEPTH)) {
+      timestamp_csv_logger->recordPreImagePublish(stream_index.first, frame, getSystemNowUs(),
-      frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
+                                                  getSteadyNowUs());
-                                                         getSystemNowUs(), getSteadyNowUs());
     }
     if (stream_index == COLOR) {
       fps_delay_status_color_->tick(frame_timestamp);
@@ -7041,8 +7098,8 @@ 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()
+  auto *timestamp_csv_logger =
-                                                     : gyro_timestamp_csv_logger_.get();
+      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) {
@@ -8189,8 +8246,8 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
         if (request->filter_param.size() > 1) {
           temporal_filter->setDiffScale(request->filter_param[0]);
           temporal_filter->setWeight(request->filter_param[1]);
-          RCLCPP_INFO_STREAM(logger_, "Set TemporalFilter params: "
+          RCLCPP_INFO_STREAM(
-                                          << "\ndiff_scale:" << request->filter_param[0]
+              logger_, "Set TemporalFilter params: " << "\ndiff_scale:" << request->filter_param[0]
                                                      << "\nweight:" << request->filter_param[1]);
           temporal_filter_diff_threshold_ = request->filter_param[0];
           temporal_filter_weight_ = request->filter_param[1];