Merge branch 'feat/subscriber-benchmark' into v2/develop

# Conflicts:
#	orbbec_camera/src/ob_camera_node.cpp
#	orbbec_camera/tools/multi_save_rgbir.cpp

orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json

@@ -1,13 +1,14 @@
 {
   "save_rgbir_params": {
-      "time_domain": "global",
-      "usb_ports": [
-          "2-1",
-          "2-3"
-      ],
-      "camera_name": [
-          "camera_01",
-          "camera_02"
-      ]
+    "time_domain": "global",
+    "stream_names": [],
+    "usb_ports": [
+      "2-1",
+      "2-3"
+    ],
+    "camera_name": [
+      "camera_01",
+      "camera_02"
+    ]
   }
-}
+}

orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp

@@ -1,4 +1,5 @@
 #include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/msg/image.hpp>
 
 #if __has_include(<message_filters/subscriber.hpp>)
@@ -33,6 +34,7 @@
 #include <opencv2/opencv.hpp>
 #include <string>
 #include <thread>
+#include <utility>
 #include <vector>
 
 using Image = sensor_msgs::msg::Image;
@@ -69,7 +71,7 @@ class ImageSyncNode : public rclcpp::Node {
     if (sync_topics_.empty()) {
       sync_topics_ = discover_image_topics();
       RCLCPP_INFO(this->get_logger(),
-                  "Parameter sync_topics is empty. Auto-discovered %zu color/depth image topics.",
+                  "Parameter sync_topics is empty. Auto-discovered %zu supported image topics.",
                   sync_topics_.size());
     } else {
       RCLCPP_INFO(this->get_logger(), "Using %zu image topics from parameter sync_topics.",
@@ -166,6 +168,24 @@ class ImageSyncNode : public rclcpp::Node {
            str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0;
   }
 
+  static const std::array<std::pair<const char *, const char *>, 7> &supported_stream_suffixes() {
+    static const std::array<std::pair<const char *, const char *>, 7> suffixes = {{
+        {"left_color", "/left_color/image_raw"},
+        {"right_color", "/right_color/image_raw"},
+        {"left_ir", "/left_ir/image_raw"},
+        {"right_ir", "/right_ir/image_raw"},
+        {"color", "/color/image_raw"},
+        {"depth", "/depth/image_raw"},
+        {"ir", "/ir/image_raw"},
+    }};
+    return suffixes;
+  }
+
+  static bool is_supported_image_topic(const std::string &topic) {
+    return std::any_of(supported_stream_suffixes().begin(), supported_stream_suffixes().end(),
+                       [&topic](const auto &entry) { return has_suffix(topic, entry.second); });
+  }
+
   static double stamp_to_seconds(const builtin_interfaces::msg::Time &stamp) {
     return static_cast<double>(stamp.sec) + static_cast<double>(stamp.nanosec) * 1e-9;
   }
@@ -180,7 +200,7 @@ class ImageSyncNode : public rclcpp::Node {
       const auto names_and_types = this->get_topic_names_and_types();
       for (const auto &entry : names_and_types) {
         const auto &topic = entry.first;
-        if (!has_suffix(topic, "/color/image_raw") && !has_suffix(topic, "/depth/image_raw")) {
+        if (!is_supported_image_topic(topic)) {
           continue;
         }
 
@@ -207,8 +227,8 @@ class ImageSyncNode : public rclcpp::Node {
   void validate_topics() {
     if (sync_topics_.empty()) {
       throw std::runtime_error(
-          "No image topics to synchronize. Set parameter sync_topics or start color/depth cameras "
-          "before this node.");
+          "No image topics to synchronize. Set parameter sync_topics or start supported camera "
+          "streams before this node.");
     }
 
     std::vector<std::string> deduplicated_topics;
@@ -233,7 +253,7 @@ class ImageSyncNode : public rclcpp::Node {
           "Official ROS message_filters::Synchronizer supports at most 9 inputs, and this example "
           "supports 1-8 image topics. Found " +
           std::to_string(sync_topics_.size()) +
-          " color/depth image topics. Please pass <= 8 topics with sync_topics or split the sync "
+          " image topics. Please pass <= 8 topics with sync_topics or split the sync "
           "into multiple stages.");
     }
   }
@@ -247,14 +267,13 @@ class ImageSyncNode : public rclcpp::Node {
       info.image_type = "image";
       info.camera_name = topic;
 
-      const auto color_pos = topic.rfind("/color/image_raw");
-      const auto depth_pos = topic.rfind("/depth/image_raw");
-      if (color_pos != std::string::npos) {
-        info.image_type = "color";
-        info.camera_name = topic.substr(0, color_pos);
-      } else if (depth_pos != std::string::npos) {
-        info.image_type = "depth";
-        info.camera_name = topic.substr(0, depth_pos);
+      for (const auto &entry : supported_stream_suffixes()) {
+        const std::string suffix = entry.second;
+        if (has_suffix(topic, suffix)) {
+          info.image_type = entry.first;
+          info.camera_name = topic.substr(0, topic.size() - suffix.size());
+          break;
+        }
       }
 
       const auto slash_pos = info.camera_name.find_last_of('/');
@@ -280,7 +299,17 @@ class ImageSyncNode : public rclcpp::Node {
     try {
       for (const auto &msg : msgs) {
         auto cv_image = cv_bridge::toCvShare(msg);
-        images.push_back(cv_image->image.clone());
+        cv::Mat image;
+        if (msg->encoding == sensor_msgs::image_encodings::RGB8) {
+          cv::cvtColor(cv_image->image, image, cv::COLOR_RGB2BGR);
+        } else if (msg->encoding == sensor_msgs::image_encodings::RGBA8) {
+          cv::cvtColor(cv_image->image, image, cv::COLOR_RGBA2BGR);
+        } else if (msg->encoding == sensor_msgs::image_encodings::BGRA8) {
+          cv::cvtColor(cv_image->image, image, cv::COLOR_BGRA2BGR);
+        } else {
+          image = cv_image->image.clone();
+        }
+        images.push_back(std::move(image));
         timestamps.push_back(stamp_to_seconds(msg->header.stamp));
       }
     } catch (cv_bridge::Exception &e) {
@@ -459,8 +488,14 @@ class ImageSyncNode : public rclcpp::Node {
         cv::applyColorMap(tmp, image, cv::COLORMAP_JET);
       } else if (images[i].channels() == 3) {
         image = images[i].clone();
+      } else if (images[i].channels() == 4) {
+        cv::cvtColor(images[i], image, cv::COLOR_BGRA2BGR);
       } else {
-        cv::cvtColor(images[i], image, cv::COLOR_GRAY2BGR);
+        RCLCPP_WARN(this->get_logger(), "Display first channel of unsupported %d-channel image %s",
+                    images[i].channels(), topic_infos[i].topic.c_str());
+        cv::Mat first_channel;
+        cv::extractChannel(images[i], first_channel, 0);
+        cv::cvtColor(first_channel, image, cv::COLOR_GRAY2BGR);
       }
 
       const std::string text = topic_infos[i].camera_name + " " + topic_infos[i].image_type +
@@ -550,10 +585,8 @@ class ImageSyncNode : public rclcpp::Node {
     const double avg_diff = diff_sum_ / count_;
 
     std::cout << "\nImage Timestamp Difference Statistics" << std::endl;
-    std::cout << "cur: " << cur << " ms"
-              << " avg: " << avg_diff << " ms"
-              << " max: " << max_diff_ << " ms"
-              << " min: " << min_diff_ << " ms" << std::endl;
+    std::cout << "cur: " << cur << " ms" << " avg: " << avg_diff << " ms" << " max: " << max_diff_
+              << " ms" << " min: " << min_diff_ << " ms" << std::endl;
 
     if (last_time_ == 0.0) {
       last_time_ = base_t;

orbbec_camera/include/orbbec_camera/fps_delay_status.hpp

@@ -20,6 +20,7 @@
 #include "orbbec_camera_msgs/msg/device_status.hpp"
 #include <chrono>
 #include <functional>
+#include <limits>
 #include <mutex>
 namespace orbbec_camera {
 class FpsDelayStatus {
@@ -58,38 +59,57 @@ class FpsDelayStatus {
   }
 
   void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
-    std::lock_guard<std::mutex> lock(mutex_);
-    msg.color_frame_rate_cur = last_fps_;
-    msg.color_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
-    msg.color_frame_rate_min = fps_min_;
-    msg.color_frame_rate_max = fps_max_;
+    fillStatus(msg.color_frame_rate_cur, msg.color_frame_rate_avg, msg.color_frame_rate_min,
+               msg.color_frame_rate_max, msg.color_delay_ms_cur, msg.color_delay_ms_avg,
+               msg.color_delay_ms_min, msg.color_delay_ms_max);
+  }
 
-    msg.color_delay_ms_cur = last_delay_ms_;
-    msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
-    msg.color_delay_ms_min = delay_min_;
-    msg.color_delay_ms_max = delay_max_;
+  void fillLeftColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.left_color_frame_rate_cur, msg.left_color_frame_rate_avg,
+               msg.left_color_frame_rate_min, msg.left_color_frame_rate_max,
+               msg.left_color_delay_ms_cur, msg.left_color_delay_ms_avg,
+               msg.left_color_delay_ms_min, msg.left_color_delay_ms_max);
+  }
 
-    last_delay_ms_ = 0.0;
-    last_fps_ = 0.0;
-    frame_count_ = 0;
-    fps_sum_ = delay_sum_ = 0.0;
-    fps_max_ = delay_max_ = 0.0;
-    fps_min_ = delay_min_ = 0.0;
+  void fillRightColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.right_color_frame_rate_cur, msg.right_color_frame_rate_avg,
+               msg.right_color_frame_rate_min, msg.right_color_frame_rate_max,
+               msg.right_color_delay_ms_cur, msg.right_color_delay_ms_avg,
+               msg.right_color_delay_ms_min, msg.right_color_delay_ms_max);
   }
 
   void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.depth_frame_rate_cur, msg.depth_frame_rate_avg, msg.depth_frame_rate_min,
+               msg.depth_frame_rate_max, msg.depth_delay_ms_cur, msg.depth_delay_ms_avg,
+               msg.depth_delay_ms_min, msg.depth_delay_ms_max);
+  }
+
+  void fillLeftIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.left_ir_frame_rate_cur, msg.left_ir_frame_rate_avg, msg.left_ir_frame_rate_min,
+               msg.left_ir_frame_rate_max, msg.left_ir_delay_ms_cur, msg.left_ir_delay_ms_avg,
+               msg.left_ir_delay_ms_min, msg.left_ir_delay_ms_max);
+  }
+
+  void fillRightIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.right_ir_frame_rate_cur, msg.right_ir_frame_rate_avg,
+               msg.right_ir_frame_rate_min, msg.right_ir_frame_rate_max, msg.right_ir_delay_ms_cur,
+               msg.right_ir_delay_ms_avg, msg.right_ir_delay_ms_min, msg.right_ir_delay_ms_max);
+  }
+
+ private:
+  void fillStatus(double &frame_rate_cur, double &frame_rate_avg, double &frame_rate_min,
+                  double &frame_rate_max, double &delay_ms_cur, double &delay_ms_avg,
+                  double &delay_ms_min, double &delay_ms_max) {
     std::lock_guard<std::mutex> lock(mutex_);
-    msg.depth_frame_rate_cur = last_fps_;
-    msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
-    msg.depth_frame_rate_min = fps_min_;
-    msg.depth_frame_rate_max = fps_max_;
+    frame_rate_cur = last_fps_;
+    frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
+    frame_rate_min = frame_count_ > 0 ? fps_min_ : 0;
+    frame_rate_max = frame_count_ > 0 ? fps_max_ : 0;
 
-    msg.depth_delay_ms_cur = last_delay_ms_;
-    msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
-    msg.depth_delay_ms_min = delay_min_;
-    msg.depth_delay_ms_max = delay_max_;
-
-    // RCLCPP_ERROR_STREAM(logger_, "Depth status: " << fps_sum_ << "," << frame_count_);
+    delay_ms_cur = last_delay_ms_;
+    delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
+    delay_ms_min = frame_count_ > 0 ? delay_min_ : 0;
+    delay_ms_max = frame_count_ > 0 ? delay_max_ : 0;
 
     last_delay_ms_ = 0.0;
     last_fps_ = 0.0;
@@ -99,7 +119,6 @@ class FpsDelayStatus {
     fps_min_ = delay_min_ = 0.0;
   }
 
- private:
   mutable std::mutex mutex_;
   u_int64_t last_stream_timestamp_{0};
   double last_delay_ms_{0.0};

orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h

@@ -21,7 +21,7 @@ namespace orbbec_camera {
 
 class FrameTimestampCsvLogger {
  public:
-  enum class OutputMode { SYNCED, COLOR, DEPTH };
+  enum class OutputMode { SYNCED, COLOR, LEFT_COLOR, RIGHT_COLOR, DEPTH, LEFT_IR, RIGHT_IR };
 
   FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
                           OutputMode output_mode, rclcpp::Logger logger);

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -237,10 +237,14 @@ class OBCameraNode {
   }
   void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
     fps_delay_status_color_->fillColorStatus(status_msg);
+    fps_delay_status_left_color_->fillLeftColorStatus(status_msg);
+    fps_delay_status_right_color_->fillRightColorStatus(status_msg);
   }
 
   void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
     fps_delay_status_depth_->fillDepthStatus(status_msg);
+    fps_delay_status_left_ir_->fillLeftIrStatus(status_msg);
+    fps_delay_status_right_ir_->fillRightIrStatus(status_msg);
   }
 
   bool checkUserCalibrationReady() {
@@ -1186,12 +1190,18 @@ class OBCameraNode {
   bool show_fps_enable_ = false;
   bool enable_publish_extrinsic_ = false;
   std::unique_ptr<FpsCounter> fps_counter_color_{nullptr};
+  std::unique_ptr<FpsCounter> fps_counter_left_color_{nullptr};
+  std::unique_ptr<FpsCounter> fps_counter_right_color_{nullptr};
   std::unique_ptr<FpsCounter> fps_counter_depth_{nullptr};
   std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr};
   std::unique_ptr<FpsCounter> fps_counter_right_ir_{nullptr};
 
   std::unique_ptr<FpsDelayStatus> fps_delay_status_color_{nullptr};
+  std::unique_ptr<FpsDelayStatus> fps_delay_status_left_color_{nullptr};
+  std::unique_ptr<FpsDelayStatus> fps_delay_status_right_color_{nullptr};
   std::unique_ptr<FpsDelayStatus> fps_delay_status_depth_{nullptr};
+  std::unique_ptr<FpsDelayStatus> fps_delay_status_left_ir_{nullptr};
+  std::unique_ptr<FpsDelayStatus> fps_delay_status_right_ir_{nullptr};
 
   std::string intra_camera_sync_reference_ = "";
   std::string ae_reference_stream_;

orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h

@@ -22,7 +22,11 @@ class TimestampCsvLogger {
     std::string csv_file_path;
     bool frame_sync_enabled = false;
     bool color_enabled = false;
+    bool left_color_enabled = false;
+    bool right_color_enabled = false;
     bool depth_enabled = false;
+    bool left_ir_enabled = false;
+    bool right_ir_enabled = false;
     bool imu_sync_enabled = false;
     bool accel_enabled = false;
     bool gyro_enabled = false;
@@ -44,6 +48,9 @@ class TimestampCsvLogger {
                            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 recordImageFrameArrival(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 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);
@@ -64,7 +71,11 @@ class TimestampCsvLogger {
   std::atomic_bool shutdown_requested_{false};
   std::unique_ptr<FrameTimestampCsvLogger> synced_image_logger_;
   std::unique_ptr<FrameTimestampCsvLogger> color_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> left_color_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> right_color_logger_;
   std::unique_ptr<FrameTimestampCsvLogger> depth_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> left_ir_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> right_ir_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> synced_imu_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> accel_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> gyro_logger_;

orbbec_camera/scripts/common_benchmark_node.py

@@ -4,11 +4,14 @@
 Support: ROS2
 name: common_benchmark_node.py
 function: A ROS2 node to monitor and log the performance of an Orbbec camera node:
-          frame rates, delays, CPU and RAM usage, packet/frame loss statistics.
+          subscriber-side frame rates, end-to-end delays, CPU and RAM usage,
+          and estimated frame loss statistics.
 usage:
     ros2 run orbbec_camera common_benchmark_node.py --run_time 20 --csv_file /tmp/cam_log.csv
                     You can also pass an ideal frame rate for drop detection: --ideal_fps 30
                     Monitor multiple cameras: --camera_names camera,camera01
+                    Select topics explicitly: --topics color,depth
+                    Compressed images and point clouds require full topic names.
 """
 
 import argparse
@@ -21,11 +24,22 @@ import csv
 import os
 from collections import defaultdict
 from orbbec_camera_msgs.msg import DeviceStatus
-from sensor_msgs.msg import Image
+from sensor_msgs.msg import CompressedImage, Image, PointCloud2
 
 from tabulate import tabulate
 
 CAMERA_NODE_NAMES = ["component_container", "orbbec_camera_node", "nodelet"]
+MONITORED_STREAMS = (
+    "color",
+    "depth",
+    "ir",
+    "left_ir",
+    "right_ir",
+    "left_color",
+    "right_color",
+)
+DISCOVERY_INTERVAL_SECONDS = 0.1
+DISCOVERY_DURATION_SECONDS = 1.0
 DOCUMENTATION_URL = (
     "https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/"
     "6_benchmark/benchmark_tools.html"
@@ -99,24 +113,62 @@ def estimate_dropped_frames(dt, expected_interval):
 # ----------------------------------------------
 
 class TopicTracker:
-    def __init__(self, logger=None):
+    def __init__(self, logger=None, sample_start_time=None):
         self.received = 0
-
-        self.last_time = None
+        self.sample_start_time = sample_start_time or time.monotonic()
+        self.last_sample_time = self.sample_start_time
+        self.last_sample_received = 0
+        self.last_header_stamp = None
+        self.estimated_interval = None
         self.drop_frames = 0
 
         self.logger = logger
 
-    def on_msg(self, header, avg_fps):
-        stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
+    def on_msg(self, header, ros_receive_time, ideal_fps):
+        """Record one received message and return its age in milliseconds."""
+        header_stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
         self.received += 1
+        delay_ms = None
 
-        if self.last_time is not None and avg_fps > 0:
-            dt = stamp - self.last_time
-            expected_interval = 1.0 / avg_fps
-            self.drop_frames += estimate_dropped_frames(dt, expected_interval)
+        if header_stamp > 0:
+            delay_ms = (ros_receive_time - header_stamp) * 1000.0
+            if self.last_header_stamp is not None:
+                header_dt = header_stamp - self.last_header_stamp
+                if header_dt > 0:
+                    expected_interval = (
+                        1.0 / ideal_fps
+                        if ideal_fps and ideal_fps > 0.0
+                        else self.estimated_interval
+                    )
+                    if expected_interval is not None:
+                        self.drop_frames += estimate_dropped_frames(
+                            header_dt, expected_interval
+                        )
+                    self.update_estimated_interval(header_dt)
+            self.last_header_stamp = header_stamp
 
-        self.last_time = stamp
+        return delay_ms
+
+    def sample_fps(self, sample_time):
+        """Calculate current and average subscriber throughput."""
+        window_elapsed = sample_time - self.last_sample_time
+        total_elapsed = sample_time - self.sample_start_time
+        if window_elapsed <= 0.0 or total_elapsed <= 0.0:
+            return None
+
+        window_received = self.received - self.last_sample_received
+        current_fps = window_received / window_elapsed
+        average_fps = self.received / total_elapsed
+        self.last_sample_time = sample_time
+        self.last_sample_received = self.received
+        return current_fps, average_fps
+
+    def update_estimated_interval(self, interval):
+        """Learn the nominal source interval while excluding likely frame gaps."""
+        if self.estimated_interval is None or interval < 0.75 * self.estimated_interval:
+            self.estimated_interval = interval
+        elif interval <= 1.5 * self.estimated_interval:
+            self.estimated_interval = 0.9 * self.estimated_interval + 0.1 * interval
 
     def frames_loss_rate(self):
         total = self.received + self.drop_frames
@@ -129,17 +181,27 @@ class TopicTracker:
 
 
 class CameraMonitorNode(Node):
-    def __init__(self, run_time, csv_file="camera_monitor_log.csv", ideal_fps: float = 0.0, camera_names=None):
+    def __init__(
+        self,
+        run_time,
+        csv_file="camera_monitor_log.csv",
+        ideal_fps: float = 0.0,
+        camera_names=None,
+        topics=None,
+    ):
         super().__init__("camera_monitor_node")
 
         self.run_time = run_time
-        self.start_time = time.time()
+        self.discovery_start_time = time.time()
+        self.discovery_start_monotonic = None
+        self.start_time = None
         self.process = psutil.Process(os.getpid())
         self.first_data_collected = False
         self.camera_names = parse_camera_names(camera_names)
         self.node_names = {camera_name: "Not Found" for camera_name in self.camera_names}
         self.total_node_name = "Not Found"
-        # If > 0, use this ideal fps value for drop-frame detection instead of the reported average
+        # If > 0, use this ideal FPS for drop detection instead of learning the
+        # nominal interval from received image timestamps.
         self.ideal_fps = float(ideal_fps) if ideal_fps is not None else 0.0
         self.finished = False
 
@@ -153,22 +215,26 @@ class CameraMonitorNode(Node):
                 "stats": defaultdict(make_stat),
                 "cpu_stats": make_stat(),
                 "ram_stats": make_stat(),
-                "trackers": {
-                    "color": TopicTracker(logger=self.get_logger()),
-                    "depth": TopicTracker(logger=self.get_logger())
-                }
+                "trackers": {},
             }
 
         self.total_cpu_stats = make_stat()
         self.total_ram_stats = make_stat()
+        self.topic_subscriptions = {}
+        self.topic_configs = {}
+        self.discovered_streams = {}
+        self.discovery_complete = False
+        self.discovery_timer = None
+        self.timer = None
+        self.requested_streams = self.parse_requested_topics(topics)
 
         # CSV
         self.csv_file = csv_file
         self.csv_fh = open(self.csv_file, "w", newline="")
         self.csv_writer = csv.writer(self.csv_fh)
-        self.csv_writer.writerow(self.build_csv_header())
 
-        # subscriptions
+        # Device status subscriptions are always present. Data subscriptions
+        # are created once after automatic discovery or explicit selection.
         for camera_name in self.camera_names:
             ns = self.camera_namespace(camera_name)
             self.create_subscription(
@@ -177,29 +243,26 @@ class CameraMonitorNode(Node):
                 lambda msg, name=camera_name: self.status_callback(msg, name),
                 5
             )
-            self.create_subscription(
-                Image,
-                f"{ns}/color/image_raw",
-                lambda msg, name=camera_name: self.image_callback(msg, name, "color"),
-                5
-            )
-            self.create_subscription(
-                Image,
-                f"{ns}/depth/image_raw",
-                lambda msg, name=camera_name: self.image_callback(msg, name, "depth"),
-                5
-            )
 
-        # timer runs every 1s to update system stats, log csv and print status
-        self.timer = self.create_timer(1.0, self.timer_callback)
+        if self.requested_streams:
+            self.finish_topic_discovery(self.requested_streams)
+        else:
+            self.discovery_start_monotonic = time.monotonic()
+            self.discovery_timer = self.create_timer(
+                DISCOVERY_INTERVAL_SECONDS, self.update_topic_discovery
+            )
 
     def timer_callback(self):
+        if not self.discovery_complete:
+            return
+
         elapsed = time.time() - self.start_time
         if elapsed > self.run_time:
             self.finish()
             rclpy.shutdown()
             return
 
+        self.update_topic_fps_stats()
         camera_sys_stats, total_cpu, total_ram, self.total_node_name = self.get_camera_stats()
         for camera_name in self.camera_names:
             camera = self.cameras[camera_name]
@@ -220,7 +283,8 @@ class CameraMonitorNode(Node):
             return
         self.finished = True
 
-        elapsed = time.time() - self.start_time
+        timer_start = self.start_time or self.discovery_start_time
+        elapsed = time.time() - timer_start
         try:
             self.csv_fh.close()
         except Exception:
@@ -231,6 +295,150 @@ class CameraMonitorNode(Node):
     def camera_namespace(self, camera_name):
         return "/" + camera_name.strip("/")
 
+    def image_topic_name(self, camera_name, stream):
+        return f"{self.camera_namespace(camera_name)}/{stream}/image_raw"
+
+    def make_raw_image_config(self, camera_name, stream):
+        return {
+            "camera_name": camera_name,
+            "topic_id": stream,
+            "topic_name": self.image_topic_name(camera_name, stream),
+            "msg_type": Image,
+        }
+
+    def parse_full_topic(self, topic_name):
+        normalized_topic = "/" + topic_name.strip("/")
+        for camera_name in self.camera_names:
+            namespace_prefix = self.camera_namespace(camera_name) + "/"
+            if not normalized_topic.startswith(namespace_prefix):
+                continue
+
+            relative_name = normalized_topic[len(namespace_prefix):]
+            for stream in MONITORED_STREAMS:
+                raw_name = f"{stream}/image_raw"
+                if relative_name == raw_name:
+                    return self.make_raw_image_config(camera_name, stream)
+                if relative_name == f"{raw_name}/compressed":
+                    return {
+                        "camera_name": camera_name,
+                        "topic_id": f"{stream}_compressed",
+                        "topic_name": normalized_topic,
+                        "msg_type": CompressedImage,
+                    }
+                if relative_name == f"{raw_name}/compressedDepth":
+                    return {
+                        "camera_name": camera_name,
+                        "topic_id": f"{stream}_compressed_depth",
+                        "topic_name": normalized_topic,
+                        "msg_type": CompressedImage,
+                    }
+
+            point_cloud_ids = {
+                "depth/points": "depth_points",
+                "depth_registered/points": "depth_registered_points",
+            }
+            if relative_name in point_cloud_ids:
+                return {
+                    "camera_name": camera_name,
+                    "topic_id": point_cloud_ids[relative_name],
+                    "topic_name": normalized_topic,
+                    "msg_type": PointCloud2,
+                }
+        return None
+
+    def parse_requested_topics(self, topics):
+        if not topics:
+            return {}
+
+        requested = str(topics).replace(";", ",").split(",")
+        selected = {}
+        for value in requested:
+            value = value.strip()
+            if not value:
+                continue
+            if value in MONITORED_STREAMS:
+                for camera_name in self.camera_names:
+                    config = self.make_raw_image_config(camera_name, value)
+                    selected[(camera_name, config["topic_id"])] = config
+                continue
+
+            config = self.parse_full_topic(value)
+            if config is None:
+                raise ValueError(
+                    f"Unsupported topic '{value}'. Specify a raw or compressed image topic, "
+                    "or a depth/points or depth_registered/points topic under a configured "
+                    "camera namespace."
+                )
+            selected[(config["camera_name"], config["topic_id"])] = config
+        return selected
+
+    def find_published_image_streams(self):
+        published_streams = {}
+        for camera_name in self.camera_names:
+            for stream in MONITORED_STREAMS:
+                config = self.make_raw_image_config(camera_name, stream)
+                key = (camera_name, config["topic_id"])
+                if self.count_publishers(config["topic_name"]) > 0:
+                    published_streams[key] = config
+        return published_streams
+
+    def update_topic_discovery(self):
+        if self.discovery_complete:
+            return
+
+        self.discovered_streams.update(self.find_published_image_streams())
+        elapsed = time.monotonic() - self.discovery_start_monotonic
+        if elapsed >= DISCOVERY_DURATION_SECONDS:
+            self.finish_topic_discovery(self.discovered_streams)
+
+    def finish_topic_discovery(self, selected_streams):
+        self.topic_configs = dict(selected_streams)
+        sample_start_time = time.monotonic()
+        for key, config in self.topic_configs.items():
+            camera_name = config["camera_name"]
+            topic_id = config["topic_id"]
+            self.cameras[camera_name]["trackers"][topic_id] = TopicTracker(
+                logger=self.get_logger(), sample_start_time=sample_start_time
+            )
+            self.topic_subscriptions[key] = self.create_subscription(
+                config["msg_type"],
+                config["topic_name"],
+                lambda msg, name=camera_name, selected_topic_id=topic_id: self.topic_callback(
+                    msg, name, selected_topic_id
+                ),
+                5,
+            )
+
+        self.csv_writer.writerow(self.build_csv_header())
+        self.csv_fh.flush()
+        self.start_time = time.time()
+        self.discovery_complete = True
+        self.timer = self.create_timer(1.0, self.timer_callback)
+        if self.discovery_timer is not None:
+            self.discovery_timer.cancel()
+
+    def topics_for_camera(self, camera_name):
+        return [
+            config
+            for config in self.topic_configs.values()
+            if config["camera_name"] == camera_name
+        ]
+
+    def update_topic_fps_stats(self):
+        sample_time = time.monotonic()
+        for config in self.topic_configs.values():
+            camera = self.cameras[config["camera_name"]]
+            topic_id = config["topic_id"]
+            fps_sample = camera["trackers"][topic_id].sample_fps(sample_time)
+            if fps_sample is None:
+                continue
+            current_fps, average_fps = fps_sample
+            self.update_sample_stat(
+                camera["stats"][f"{topic_id}_fps"],
+                current_fps,
+                average=average_fps,
+            )
+
     def cmdline_has_camera_namespace(self, cmdline_args, camera_name):
         ns = self.camera_namespace(camera_name)
         candidates = [
@@ -311,32 +519,30 @@ class CameraMonitorNode(Node):
 
         camera["prev_online"] = msg.device_online
 
-        # update stats from DeviceStatus message fields
-        self.update_stats(camera["stats"], "color_fps", msg.color_frame_rate_cur, msg.color_frame_rate_min, msg.color_frame_rate_max, msg.color_frame_rate_avg)
-        self.update_stats(camera["stats"], "color_delay", msg.color_delay_ms_cur, msg.color_delay_ms_min, msg.color_delay_ms_max, msg.color_delay_ms_avg)
-        self.update_stats(camera["stats"], "depth_fps", msg.depth_frame_rate_cur, msg.depth_frame_rate_min, msg.depth_frame_rate_max, msg.depth_frame_rate_avg)
-        self.update_stats(camera["stats"], "depth_delay", msg.depth_delay_ms_cur, msg.depth_delay_ms_min, msg.depth_delay_ms_max, msg.depth_delay_ms_avg)
-
-    def image_callback(self, msg: Image, camera_name: str, stream: str):
-        if stream not in ("color", "depth"):
-            return
-        header = msg.header
+    def topic_callback(self, msg, camera_name: str, topic_id: str):
+        self.first_data_collected = True
         camera = self.cameras[camera_name]
-        tracker = camera["trackers"][stream]
-        # Prefer a user-specified ideal fps for drop detection when provided.
-        fps_to_use = self.ideal_fps if (self.ideal_fps and self.ideal_fps > 0.0) else camera["stats"][f"{stream}_fps"]["avg"]
-        tracker.on_msg(header, fps_to_use)
+        camera["data_collected"] = True
+        tracker = camera["trackers"][topic_id]
+        ros_receive_time = self.get_clock().now().nanoseconds * 1e-9
+        delay_ms = tracker.on_msg(
+            msg.header,
+            ros_receive_time,
+            self.ideal_fps,
+        )
 
-    def update_stats(self, stats, key, cur, min_val, max_val, avg_val):
-        if min_val <= 1e-3 or avg_val < 0:  # ignore invalid data
-            return
-        s = stats[key]
-        s["cur"] = (cur)
-        s["count"] += 1
-        s["sum"] += avg_val
-        s["avg"] = s["sum"] / s["count"] if s["count"] > 0 else 0.0
-        s["min"] = min(s["min"], min_val)
-        s["max"] = max(s["max"], max_val)
+        if delay_ms is not None:
+            self.update_sample_stat(
+                camera["stats"][f"{topic_id}_delay"], delay_ms
+            )
+
+    def update_sample_stat(self, stat, value, average=None):
+        stat["cur"] = value
+        stat["count"] += 1
+        stat["sum"] += value
+        stat["avg"] = average if average is not None else stat["sum"] / stat["count"]
+        stat["min"] = min(stat["min"], value)
+        stat["max"] = max(stat["max"], value)
 
     def update_sys_stat(self, stat_dict, value, online=True):
         stat_dict["cur"] = value
@@ -353,7 +559,7 @@ class CameraMonitorNode(Node):
         row = [round(elapsed, 2)]
         for camera_name in self.camera_names:
             camera = self.cameras[camera_name]
-            row.extend(self.build_camera_csv_values(camera))
+            row.extend(self.build_camera_csv_values(camera_name, camera))
 
         row.extend([
             round(self.total_cpu_stats["cur"], 2), round(self.total_cpu_stats["avg"], 2),
@@ -365,19 +571,42 @@ class CameraMonitorNode(Node):
 
     def build_csv_header(self):
         header = ["time(s)"]
-        camera_fields = [
-            "connection_type", "status_online", "disconnects",
-            "color_fps_cur", "color_fps_avg", "color_fps_min", "color_fps_max",
-            "color_delay_cur", "color_delay_avg", "color_delay_min", "color_delay_max",
-            "depth_fps_cur", "depth_fps_avg", "depth_fps_min", "depth_fps_max",
-            "depth_delay_cur", "depth_delay_avg", "depth_delay_min", "depth_delay_max",
-            "cpu_cur", "cpu_avg", "cpu_min", "cpu_max",
-            "ram_cur", "ram_avg", "ram_min", "ram_max",
-            "color_frames_loss", "color_frames_loss_rate(%)",
-            "depth_frames_loss", "depth_frames_loss_rate(%)"
-        ]
         for camera_name in self.camera_names:
-            header.extend([f"{camera_name}_{field}" for field in camera_fields])
+            header.extend(
+                [
+                    f"{camera_name}_connection_type",
+                    f"{camera_name}_status_online",
+                    f"{camera_name}_disconnects",
+                ]
+            )
+            for config in self.topics_for_camera(camera_name):
+                topic_id = config["topic_id"]
+                header.extend(
+                    [
+                        f"{camera_name}_{topic_id}_fps_cur",
+                        f"{camera_name}_{topic_id}_fps_avg",
+                        f"{camera_name}_{topic_id}_fps_min",
+                        f"{camera_name}_{topic_id}_fps_max",
+                        f"{camera_name}_{topic_id}_delay_cur",
+                        f"{camera_name}_{topic_id}_delay_avg",
+                        f"{camera_name}_{topic_id}_delay_min",
+                        f"{camera_name}_{topic_id}_delay_max",
+                        f"{camera_name}_{topic_id}_sub_lost_count",
+                        f"{camera_name}_{topic_id}_sub_lost_rate(%)",
+                    ]
+                )
+            header.extend(
+                [
+                    f"{camera_name}_cpu_cur",
+                    f"{camera_name}_cpu_avg",
+                    f"{camera_name}_cpu_min",
+                    f"{camera_name}_cpu_max",
+                    f"{camera_name}_ram_cur",
+                    f"{camera_name}_ram_avg",
+                    f"{camera_name}_ram_min",
+                    f"{camera_name}_ram_max",
+                ]
+            )
 
         header.extend([
             "total_cpu_cur", "total_cpu_avg", "total_cpu_min", "total_cpu_max",
@@ -385,10 +614,7 @@ class CameraMonitorNode(Node):
         ])
         return header
 
-    def build_camera_csv_values(self, camera):
-        color_tracker = camera["trackers"]["color"]
-        depth_tracker = camera["trackers"]["depth"]
-
+    def build_camera_csv_values(self, camera_name, camera):
         def safe(k):
             v = camera["stats"].get(k, {})
             return (
@@ -398,29 +624,47 @@ class CameraMonitorNode(Node):
                 self.format_csv_number(v.get("max", 0.0)),
             )
 
-        if not camera["prev_online"]:
-            return [
-                camera["connection_type"], camera["prev_online"], camera["disconnect_count"],
-                *["N/A"] * 16,
-                round(camera["cpu_stats"]["cur"], 2), "N/A", "N/A", "N/A",
-                round(camera["ram_stats"]["cur"], 2), "N/A", "N/A", "N/A",
-                color_tracker.drop_frames, round(color_tracker.frames_loss_rate() * 100.0, 3),
-                depth_tracker.drop_frames, round(depth_tracker.frames_loss_rate() * 100.0, 3)
-            ]
-
-        return [
-            camera["connection_type"], camera["prev_online"], camera["disconnect_count"],
-            *safe("color_fps"),
-            *safe("color_delay"),
-            *safe("depth_fps"),
-            *safe("depth_delay"),
-            round(camera["cpu_stats"]["cur"], 2), round(camera["cpu_stats"]["avg"], 2),
-            self.format_csv_number(camera["cpu_stats"]["min"]), self.format_csv_number(camera["cpu_stats"]["max"]),
-            round(camera["ram_stats"]["cur"], 2), round(camera["ram_stats"]["avg"], 2),
-            self.format_csv_number(camera["ram_stats"]["min"]), self.format_csv_number(camera["ram_stats"]["max"]),
-            color_tracker.drop_frames, round(color_tracker.frames_loss_rate() * 100.0, 3),
-            depth_tracker.drop_frames, round(depth_tracker.frames_loss_rate() * 100.0, 3)
+        values = [
+            camera["connection_type"],
+            camera["prev_online"],
+            camera["disconnect_count"],
         ]
+        for config in self.topics_for_camera(camera_name):
+            topic_id = config["topic_id"]
+            tracker = camera["trackers"][topic_id]
+            if camera["prev_online"]:
+                values.extend(safe(f"{topic_id}_fps"))
+                values.extend(safe(f"{topic_id}_delay"))
+            else:
+                values.extend(["N/A"] * 8)
+            values.extend(
+                [
+                    tracker.drop_frames,
+                    round(tracker.frames_loss_rate() * 100.0, 3),
+                ]
+            )
+
+        if camera["prev_online"]:
+            values.extend(
+                [
+                    round(camera["cpu_stats"]["cur"], 2),
+                    round(camera["cpu_stats"]["avg"], 2),
+                    self.format_csv_number(camera["cpu_stats"]["min"]),
+                    self.format_csv_number(camera["cpu_stats"]["max"]),
+                    round(camera["ram_stats"]["cur"], 2),
+                    round(camera["ram_stats"]["avg"], 2),
+                    self.format_csv_number(camera["ram_stats"]["min"]),
+                    self.format_csv_number(camera["ram_stats"]["max"]),
+                ]
+            )
+        else:
+            values.extend(
+                [
+                    round(camera["cpu_stats"]["cur"], 2), "N/A", "N/A", "N/A",
+                    round(camera["ram_stats"]["cur"], 2), "N/A", "N/A", "N/A",
+                ]
+            )
+        return values
 
     def format_csv_number(self, value):
         if value == float("inf") or value == float("-inf"):
@@ -436,25 +680,30 @@ class CameraMonitorNode(Node):
         rows = []
         for camera_name in self.camera_names:
             camera = self.cameras[camera_name]
-            for stream in ["color", "depth"]:
-                fps_key = f"{stream}_fps"
-                delay_key = f"{stream}_delay"
-                topic_name = f"/{camera_name}/{stream}/image_raw"
+            for config in self.topics_for_camera(camera_name):
+                topic_id = config["topic_id"]
+                fps_key = f"{topic_id}_fps"
+                delay_key = f"{topic_id}_delay"
+                topic_name = config["topic_name"]
                 if not camera["prev_online"]:
                     rows.append([camera_name, topic_name, *["N/A"] * 10])
                 else:
                     fps_vals = format_stats(camera["stats"][fps_key])
                     delay_vals = format_stats(camera["stats"][delay_key])
-                    tracker = camera["trackers"][stream]
+                    tracker = camera["trackers"][topic_id]
 
                     frames_loss = tracker.drop_frames
                     frames_loss_rate = round(tracker.frames_loss_rate() * 100.0, 3)
                     rows.append([camera_name, topic_name, *fps_vals, *delay_vals, frames_loss, frames_loss_rate])
 
-        header_bottom = ["Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max", "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)", "Pub_lost_count", "Pub_lost_rate(%)"]
+        header_bottom = [
+            "Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max",
+            "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)",
+            "Sub_lost_count", "Sub_lost_rate(%)",
+        ]
 
         os.system("clear")
-        print("Orbbec Camera Benchmark\n")
+        print("Orbbec Camera Subscriber Benchmark\n")
         print(tabulate([header_bottom] + rows, tablefmt="fancy_grid"))
 
         sys_rows = []
@@ -491,13 +740,36 @@ def main(argv=None):
     )
     parser.add_argument("--run_time", type=str, default="10s", help="Total run time for monitoring, e.g., 10s, 5m, 1h.")
     parser.add_argument("--csv_file", type=str, default="camera_monitor_log.csv")
-    parser.add_argument("--ideal_fps", type=float, default=0.0, help="Optional ideal frame rate to use for drop detection (overrides reported avg).")
+    parser.add_argument(
+        "--ideal_fps",
+        type=float,
+        default=0.0,
+        help=(
+            "Optional ideal frame rate for subscriber-side drop detection; "
+            "otherwise it is learned from image timestamps."
+        ),
+    )
     parser.add_argument("--camera_names", type=str, default="camera", help="Comma-separated camera namespaces, e.g., camera,camera01,camera02.")
+    parser.add_argument(
+        "--topics",
+        type=str,
+        default="",
+        help=(
+            "Comma-separated raw stream names or full raw/compressed image and point cloud "
+            "topics. Automatic discovery only selects raw image topics."
+        ),
+    )
     cli_args, _ = parser.parse_known_args(argv)
 
     rclpy.init(args=argv)
     run_time = parse_duration(cli_args.run_time)
-    node = CameraMonitorNode(run_time, cli_args.csv_file, ideal_fps=cli_args.ideal_fps, camera_names=cli_args.camera_names)
+    node = CameraMonitorNode(
+        run_time,
+        cli_args.csv_file,
+        ideal_fps=cli_args.ideal_fps,
+        camera_names=cli_args.camera_names,
+        topics=cli_args.topics,
+    )
 
     try:
         rclpy.spin(node)

orbbec_camera/src/frame_timestamp_csv_logger.cpp

@@ -31,6 +31,47 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
   return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
 }
 
+std::optional<FrameTimestampCsvLogger::OutputMode> outputModeForStream(OBStreamType stream_type) {
+  using OutputMode = FrameTimestampCsvLogger::OutputMode;
+  switch (stream_type) {
+    case OB_STREAM_COLOR:
+      return OutputMode::COLOR;
+    case OB_STREAM_COLOR_LEFT:
+      return OutputMode::LEFT_COLOR;
+    case OB_STREAM_COLOR_RIGHT:
+      return OutputMode::RIGHT_COLOR;
+    case OB_STREAM_DEPTH:
+      return OutputMode::DEPTH;
+    case OB_STREAM_IR_LEFT:
+      return OutputMode::LEFT_IR;
+    case OB_STREAM_IR_RIGHT:
+      return OutputMode::RIGHT_IR;
+    default:
+      return std::nullopt;
+  }
+}
+
+const char *outputModeName(FrameTimestampCsvLogger::OutputMode output_mode) {
+  using OutputMode = FrameTimestampCsvLogger::OutputMode;
+  switch (output_mode) {
+    case OutputMode::COLOR:
+      return "color";
+    case OutputMode::LEFT_COLOR:
+      return "left_color";
+    case OutputMode::RIGHT_COLOR:
+      return "right_color";
+    case OutputMode::DEPTH:
+      return "depth";
+    case OutputMode::LEFT_IR:
+      return "left_ir";
+    case OutputMode::RIGHT_IR:
+      return "right_ir";
+    case OutputMode::SYNCED:
+      return "synced";
+  }
+  return "unknown";
+}
+
 }  // namespace
 
 FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
@@ -104,9 +145,8 @@ 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) ||
-      (stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) ||
-      (stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) {
+  const auto expected_output_mode = outputModeForStream(stream_type);
+  if (!enabled_ || !frame || !expected_output_mode || output_mode_ != *expected_output_mode) {
     return;
   }
   recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -163,14 +203,11 @@ void FrameTimestampCsvLogger::shutdown() {
 
 FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream(
     OBStreamType stream_type) const {
-  if (stream_type == OB_STREAM_COLOR) {
-    return TrackedStream::COLOR;
-  }
-  return TrackedStream::DEPTH;
+  return stream_type == OB_STREAM_DEPTH ? TrackedStream::DEPTH : TrackedStream::COLOR;
 }
 
 bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const {
-  return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH;
+  return outputModeForStream(stream_type).has_value();
 }
 
 void FrameTimestampCsvLogger::recordFrameSetInternal(
@@ -294,10 +331,12 @@ void FrameTimestampCsvLogger::completeImagePublishInternal(
     auto row_id_it = row_map.find(frame_index);
     if (row_id_it == row_map.end()) {
       if (publish_system_us.has_value()) {
-        RCLCPP_WARN_STREAM(logger_,
-                           "Frame timestamp CSV logger missed row mapping for stream "
-                               << (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
-                               << " frame index " << frame_index);
+        RCLCPP_WARN_STREAM(
+            logger_, "Frame timestamp CSV logger missed row mapping for stream "
+                         << (output_mode_ == OutputMode::SYNCED
+                                 ? (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
+                                 : outputModeName(output_mode_))
+                         << " frame index " << frame_index);
       }
       return;
     }
@@ -357,7 +396,10 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
         previous.dropped_frames += lost_frames;
         RCLCPP_WARN_STREAM(logger_,
                            "Frame drop detected: stage=SDK_RECEIVE"
-                               << " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
+                               << " stream="
+                               << (output_mode_ == OutputMode::SYNCED
+                                       ? (stream == TrackedStream::COLOR ? "color" : "depth")
+                                       : outputModeName(output_mode_))
                                << " frame_index=" << state.frame_index
                                << " dropped=" << previous.dropped_frames);
       }
@@ -408,7 +450,10 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
         previous.publish_dropped_frames += lost_frames;
         RCLCPP_WARN_STREAM(logger_,
                            "Frame drop detected: stage=ROS_PUBLISH"
-                               << " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
+                               << " stream="
+                               << (output_mode_ == OutputMode::SYNCED
+                                       ? (stream == TrackedStream::COLOR ? "color" : "depth")
+                                       : outputModeName(output_mode_))
                                << " frame_index=" << state.frame_index
                                << " dropped=" << previous.publish_dropped_frames);
       }
@@ -485,12 +530,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);
   }
+  if (output_mode_ != OutputMode::SYNCED) {
+    return serializeStreamColumns(row.color);
+  }
   std::ostringstream ss;
   ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
   return ss.str();
@@ -560,14 +605,12 @@ std::string FrameTimestampCsvLogger::csvHeader() const {
     ss << prefix << "_sdk_delay_from_global_us,";
     ss << prefix << "_sdk_delay_from_system_us";
   };
-  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) {
-    append_stream_header("color");
-  }
   if (output_mode_ == OutputMode::SYNCED) {
+    append_stream_header("color");
     ss << ",";
-  }
-  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
     append_stream_header("depth");
+  } else {
+    append_stream_header(outputModeName(output_mode_));
   }
   return ss.str();
 }
@@ -641,10 +684,8 @@ void FrameTimestampCsvLogger::writerThreadMain() {
 std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
   const std::filesystem::path original_path(csv_file_path_);
   std::string suffix;
-  if (output_mode_ == OutputMode::COLOR) {
-    suffix = "_color";
-  } else if (output_mode_ == OutputMode::DEPTH) {
-    suffix = "_depth";
+  if (output_mode_ != OutputMode::SYNCED) {
+    suffix = "_" + std::string(outputModeName(output_mode_));
   }
 
   auto indexed_filename = original_path.stem().string() + suffix;

orbbec_camera/src/ob_camera_node.cpp

@@ -743,7 +743,11 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
     timestamp_config.csv_file_path = frame_timestamp_csv_file_;
     timestamp_config.frame_sync_enabled = enable_frame_sync_;
     timestamp_config.color_enabled = enable_stream_[COLOR];
+    timestamp_config.left_color_enabled = enable_stream_[COLOR_LEFT];
+    timestamp_config.right_color_enabled = enable_stream_[COLOR_RIGHT];
     timestamp_config.depth_enabled = enable_stream_[DEPTH];
+    timestamp_config.left_ir_enabled = enable_stream_[INFRA1];
+    timestamp_config.right_ir_enabled = enable_stream_[INFRA2];
     timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
     timestamp_config.accel_enabled = enable_stream_[ACCEL];
     timestamp_config.gyro_enabled = enable_stream_[GYRO];
@@ -763,6 +767,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   is_camera_node_initialized_ = true;
 
   fps_counter_color_ = std::make_unique<FpsCounter>("Color", logger_, 1);
+  fps_counter_left_color_ = std::make_unique<FpsCounter>("Left Color", logger_, 1);
+  fps_counter_right_color_ = std::make_unique<FpsCounter>("Right Color", logger_, 1);
   fps_counter_depth_ = std::make_unique<FpsCounter>("Depth", logger_, 1);
   fps_counter_left_ir_ = std::make_unique<FpsCounter>("Left Ir", logger_, 1);
   fps_counter_right_ir_ = std::make_unique<FpsCounter>("Right Ir", logger_, 1);
@@ -772,12 +778,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
     log_level = LogLevel::INFO;
   }
   fps_counter_color_->setLogLevel(log_level);
+  fps_counter_left_color_->setLogLevel(log_level);
+  fps_counter_right_color_->setLogLevel(log_level);
   fps_counter_depth_->setLogLevel(log_level);
   fps_counter_left_ir_->setLogLevel(log_level);
   fps_counter_right_ir_->setLogLevel(log_level);
 
   fps_delay_status_color_ = std::make_unique<FpsDelayStatus>(logger_);
+  fps_delay_status_left_color_ = std::make_unique<FpsDelayStatus>(logger_);
+  fps_delay_status_right_color_ = std::make_unique<FpsDelayStatus>(logger_);
   fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>(logger_);
+  fps_delay_status_left_ir_ = std::make_unique<FpsDelayStatus>(logger_);
+  fps_delay_status_right_ir_ = std::make_unique<FpsDelayStatus>(logger_);
 }
 
 template <class T>
@@ -3237,7 +3249,7 @@ void OBCameraNode::setupLeftIrPostProcessFilter() {
   }
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info);
-  if (isGemini330SeriesPID(pid_)) {
+  if (isGemini330SeriesPID(pid_) || isGemini305SeriesPID(pid_)) {
     auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT);
     left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters();
     if (left_ir_filter_list_.empty()) {
@@ -3278,7 +3290,7 @@ void OBCameraNode::setupRightIrPostProcessFilter() {
   }
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info);
-  if (isGemini330SeriesPID(pid_)) {
+  if (isGemini330SeriesPID(pid_) || isGemini305SeriesPID(pid_)) {
     auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT);
     right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters();
     if (right_ir_filter_list_.empty()) {
@@ -3537,35 +3549,44 @@ void OBCameraNode::selectBaseStream() {
 
 void OBCameraNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor) {
   auto profiles = sensor->getStreamProfileList();
+  const auto sensor_type = sensor->getType();
   for (size_t i = 0; i < profiles->getCount(); i++) {
     auto origin_profile = profiles->getProfile(i);
-    if (sensor->getType() == OB_SENSOR_COLOR) {
+    if (sensor_type == OB_SENSOR_COLOR || sensor_type == OB_SENSOR_COLOR_LEFT ||
+        sensor_type == OB_SENSOR_COLOR_RIGHT) {
       auto profile = origin_profile->as<ob::VideoStreamProfile>();
-      RCLCPP_INFO_STREAM(
-          logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
-                                     << profile->getFps() << "fps " << profile->getFormat());
-    } else if (sensor->getType() == OB_SENSOR_DEPTH) {
+      const char *stream_name = sensor_type == OB_SENSOR_COLOR_LEFT    ? "left_color"
+                                : sensor_type == OB_SENSOR_COLOR_RIGHT ? "right_color"
+                                                                       : "color";
+      RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x"
+                                              << profile->getHeight() << " " << profile->getFps()
+                                              << "fps " << profile->getFormat());
+    } else if (sensor_type == OB_SENSOR_DEPTH) {
       auto profile = origin_profile->as<ob::VideoStreamProfile>();
       RCLCPP_INFO_STREAM(
           logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
                                      << profile->getFps() << "fps " << profile->getFormat());
-    } else if (sensor->getType() == OB_SENSOR_IR) {
+    } else if (sensor_type == OB_SENSOR_IR || sensor_type == OB_SENSOR_IR_LEFT ||
+               sensor_type == OB_SENSOR_IR_RIGHT) {
       auto profile = origin_profile->as<ob::VideoStreamProfile>();
-      RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x"
-                                                 << profile->getHeight() << " " << profile->getFps()
-                                                 << "fps " << profile->getFormat());
-    } else if (sensor->getType() == OB_SENSOR_ACCEL) {
+      const char *stream_name = sensor_type == OB_SENSOR_IR_LEFT    ? "left_ir"
+                                : sensor_type == OB_SENSOR_IR_RIGHT ? "right_ir"
+                                                                    : "ir";
+      RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x"
+                                              << profile->getHeight() << " " << profile->getFps()
+                                              << "fps " << profile->getFormat());
+    } else if (sensor_type == OB_SENSOR_ACCEL) {
       auto profile = origin_profile->as<ob::AccelStreamProfile>();
       RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->getSampleRate()
                                                                << "  full scale_range "
                                                                << profile->getFullScaleRange());
-    } else if (sensor->getType() == OB_SENSOR_GYRO) {
+    } else if (sensor_type == OB_SENSOR_GYRO) {
       auto profile = origin_profile->as<ob::GyroStreamProfile>();
       RCLCPP_INFO_STREAM(logger_, "gyro profile: sampleRate " << profile->getSampleRate()
                                                               << "  full scale_range "
                                                               << profile->getFullScaleRange());
     } else {
-      RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->getType()));
+      RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor_type));
     }
   }
 }
@@ -3667,11 +3688,11 @@ void OBCameraNode::setupProfiles() {
       if (selected_profile->format() == OB_FORMAT_BGRA) {
         images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
         encoding_[elem] = sensor_msgs::image_encodings::BGRA8;
-        unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
+        unit_step_size_[elem] = 4 * sizeof(uint8_t);
       } else if (selected_profile->format() == OB_FORMAT_RGBA) {
         images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
         encoding_[elem] = sensor_msgs::image_encodings::RGBA8;
-        unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
+        unit_step_size_[elem] = 4 * sizeof(uint8_t);
       } else {
         images_[elem] =
             cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
@@ -6501,6 +6522,20 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
         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);
+
+    const auto record_side_stream = [&](const stream_index_pair &stream_index,
+                                        OBFrameType frame_type) {
+      auto frame = frame_set->getFrame(frame_type);
+      if (enable_stream_[stream_index] && frame) {
+        timestamp_csv_logger_->recordImageFrameArrival(stream_index.first, frame,
+                                                       frame_set_arrival_system_us,
+                                                       frame_set_arrival_steady_us, true);
+      }
+    };
+    record_side_stream(COLOR_LEFT, OB_FRAME_COLOR_LEFT);
+    record_side_stream(COLOR_RIGHT, OB_FRAME_COLOR_RIGHT);
+    record_side_stream(INFRA1, OB_FRAME_IR_LEFT);
+    record_side_stream(INFRA2, OB_FRAME_IR_RIGHT);
   }
 
   try {
@@ -6542,10 +6577,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
       setColorAutoExposureROI();
       left_color_frame = processColorFrameFilter(left_color_frame);
       frame_set->pushFrame(left_color_frame);
+      fps_counter_left_color_->tick();
     }
     if (right_color_frame) {
       right_color_frame = processColorFrameFilter(right_color_frame);
       frame_set->pushFrame(right_color_frame);
+      fps_counter_right_color_->tick();
     }
     if (left_ir_frame) {
       left_ir_frame = processLeftIrFrameFilter(left_ir_frame);
@@ -7065,6 +7102,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) ==
           interleave_skip_index_) {
         RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
+        record_image_publish_skipped();
         return;
       }
     }
@@ -7147,13 +7185,19 @@ 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 && log_image_timestamps) {
+    if (!has_raw_image_subscriber && log_image_timestamps) {
       timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
                                                    getSteadyNowUs());
     }
     publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
-    if (!has_raw_image_subscriber && stream_index == COLOR) {
-      fps_delay_status_color_->tick(frame_timestamp);
+    if (!has_raw_image_subscriber) {
+      if (stream_index == COLOR) {
+        fps_delay_status_color_->tick(frame_timestamp);
+      } else if (stream_index == COLOR_LEFT) {
+        fps_delay_status_left_color_->tick(frame_timestamp);
+      } else {
+        fps_delay_status_right_color_->tick(frame_timestamp);
+      }
     }
   }
 
@@ -7178,10 +7222,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     }
     if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
       RCLCPP_ERROR(logger_, "left color frame is not decoded");
+      record_image_publish_skipped();
       return;
     }
     if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
       RCLCPP_ERROR(logger_, "right color frame is not decoded");
+      record_image_publish_skipped();
       return;
     }
     if (frame->getType() == OB_FRAME_COLOR) {
@@ -7241,8 +7287,16 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     }
     if (stream_index == COLOR) {
       fps_delay_status_color_->tick(frame_timestamp);
+    } else if (stream_index == COLOR_LEFT) {
+      fps_delay_status_left_color_->tick(frame_timestamp);
+    } else if (stream_index == COLOR_RIGHT) {
+      fps_delay_status_right_color_->tick(frame_timestamp);
     } else if (stream_index == DEPTH) {
       fps_delay_status_depth_->tick(frame_timestamp);
+    } else if (stream_index == INFRA1) {
+      fps_delay_status_left_ir_->tick(frame_timestamp);
+    } else if (stream_index == INFRA2) {
+      fps_delay_status_right_ir_->tick(frame_timestamp);
     }
     image_publishers_[stream_index]->publish(std::move(image_msg));
   }

orbbec_camera/src/timestamp_csv_logger.cpp

@@ -29,6 +29,18 @@ TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger)
       depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH);
     }
   }
+  if (config.left_color_enabled) {
+    left_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_COLOR);
+  }
+  if (config.right_color_enabled) {
+    right_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_COLOR);
+  }
+  if (config.left_ir_enabled) {
+    left_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_IR);
+  }
+  if (config.right_ir_enabled) {
+    right_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_IR);
+  }
 
   if (config.csv_file_path.empty()) {
     return;
@@ -64,7 +76,12 @@ bool TimestampCsvLogger::enabled() const {
 
 bool TimestampCsvLogger::imageEnabled() const {
   return (synced_image_logger_ && synced_image_logger_->enabled()) ||
-         (color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled());
+         (color_logger_ && color_logger_->enabled()) ||
+         (left_color_logger_ && left_color_logger_->enabled()) ||
+         (right_color_logger_ && right_color_logger_->enabled()) ||
+         (depth_logger_ && depth_logger_->enabled()) ||
+         (left_ir_logger_ && left_ir_logger_->enabled()) ||
+         (right_ir_logger_ && right_ir_logger_->enabled());
 }
 
 bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const {
@@ -104,6 +121,18 @@ void TimestampCsvLogger::recordImageFrameSet(const std::shared_ptr<ob::Frame> &c
   }
 }
 
+void TimestampCsvLogger::recordImageFrameArrival(OBStreamType stream_type,
+                                                 const std::shared_ptr<ob::Frame> &frame,
+                                                 int64_t arrival_system_us,
+                                                 int64_t arrival_steady_us,
+                                                 bool image_publish_expected) {
+  auto *timestamp_logger = imageLoggerForStream(stream_type);
+  if (timestamp_logger) {
+    timestamp_logger->recordStandaloneFrameArrival(stream_type, frame, arrival_system_us,
+                                                   arrival_steady_us, image_publish_expected);
+  }
+}
+
 void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type,
                                                const std::shared_ptr<ob::Frame> &frame,
                                                int64_t publish_system_us,
@@ -164,7 +193,11 @@ void TimestampCsvLogger::shutdown() noexcept {
 
   shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger");
   shutdown_logger(color_logger_, "color timestamp CSV logger");
+  shutdown_logger(left_color_logger_, "left color timestamp CSV logger");
+  shutdown_logger(right_color_logger_, "right color timestamp CSV logger");
   shutdown_logger(depth_logger_, "depth timestamp CSV logger");
+  shutdown_logger(left_ir_logger_, "left IR timestamp CSV logger");
+  shutdown_logger(right_ir_logger_, "right IR timestamp CSV logger");
   shutdown_logger(synced_imu_logger_, "synced IMU timestamp CSV logger");
   shutdown_logger(accel_logger_, "accel timestamp CSV logger");
   shutdown_logger(gyro_logger_, "gyro timestamp CSV logger");
@@ -177,6 +210,18 @@ FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType s
   if (stream_type == OB_STREAM_DEPTH) {
     return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get();
   }
+  if (stream_type == OB_STREAM_COLOR_LEFT) {
+    return left_color_logger_.get();
+  }
+  if (stream_type == OB_STREAM_COLOR_RIGHT) {
+    return right_color_logger_.get();
+  }
+  if (stream_type == OB_STREAM_IR_LEFT) {
+    return left_ir_logger_.get();
+  }
+  if (stream_type == OB_STREAM_IR_RIGHT) {
+    return right_ir_logger_.get();
+  }
   return nullptr;
 }
 

orbbec_camera/tools/list_camera_profile.cpp

@@ -154,8 +154,11 @@ void listSensorProfiles(const std::shared_ptr<ob::Device>& device) {
                   << " | width: " << profile->getDecimationConfig().originWidth
                   << " height: " << profile->getDecimationConfig().originHeight
                   << " downscale:" << profile->getDecimationConfig().factor << std::endl;
-      } else if (sensor->getType() == OB_SENSOR_COLOR || sensor->getType() == OB_SENSOR_DEPTH ||
-                 sensor->getType() == OB_SENSOR_IR || sensor->getType() == OB_SENSOR_IR_LEFT ||
+      } else if (sensor->getType() == OB_SENSOR_COLOR ||
+                 sensor->getType() == OB_SENSOR_COLOR_LEFT ||
+                 sensor->getType() == OB_SENSOR_COLOR_RIGHT ||
+                 sensor->getType() == OB_SENSOR_DEPTH || sensor->getType() == OB_SENSOR_IR ||
+                 sensor->getType() == OB_SENSOR_IR_LEFT ||
                  sensor->getType() == OB_SENSOR_IR_RIGHT) {
         auto profile = origin_profile->as<ob::VideoStreamProfile>();
         std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getWidth()

orbbec_camera/tools/multi_save_rgbir.cpp

@@ -1,72 +1,114 @@
-
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
-#include <orbbec_camera/ob_camera_node_driver.h>
-#include <orbbec_camera/utils.h>
-#include "orbbec_camera/ob_camera_node.h"
-#include "orbbec_camera_msgs/msg/metadata.hpp"
-#include <std_msgs/msg/int32.hpp>
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ctime>
 #include <filesystem>
-#include <regex>
+#include <fstream>
+#include <functional>
+#include <iomanip>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <nlohmann/json.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <std_msgs/msg/header.hpp>
+
+#include "orbbec_camera/ob_camera_node.h"
+#include "orbbec_camera/utils.h"
+#include "orbbec_camera_msgs/msg/metadata.hpp"
+
 namespace orbbec_camera {
 namespace tools {
-struct ImageMetadata {
-  std::vector<std::vector<std::string>> exposure_buffs;
-  std::vector<std::vector<std::string>> gain_buffs;
+namespace {
+
+const std::array<std::string, 6> kSupportedStreamNames = {
+    "color", "left_color", "right_color", "ir", "left_ir", "right_ir",
+};
+constexpr auto kStreamDiscoveryPollInterval = std::chrono::milliseconds(100);
+constexpr auto kStreamDiscoveryStablePeriod = std::chrono::seconds(1);
+constexpr auto kStreamDiscoveryTimeout = std::chrono::seconds(5);
+constexpr size_t kMinimumPendingFrameLimit = 30;
+
+struct StreamTopicInfo {
+  std::string name;
+  bool metadata_available = false;
+
+  bool operator==(const StreamTopicInfo &other) const {
+    return name == other.name && metadata_available == other.metadata_available;
+  }
+};
+
+bool isSupportedStreamName(const std::string &stream_name) {
+  return std::find(kSupportedStreamNames.begin(), kSupportedStreamNames.end(), stream_name) !=
+         kSupportedStreamNames.end();
+}
+
+bool isColorCaptureStreamName(const std::string &stream_name) {
+  return stream_name == "color" || stream_name == "left_color" || stream_name == "right_color";
+}
+
+std::string cameraNamespace(const std::string &camera_name) {
+  if (!camera_name.empty() && camera_name.front() == '/') {
+    return camera_name;
+  }
+  return "/" + camera_name;
+}
+
+}  // namespace
+
+struct StreamCapture {
+  struct FrameMetadata {
+    std::string exposure;
+    std::string gain;
+  };
+
+  struct PendingImage {
+    cv::Mat image;
+    std::string current_timestamp;
+    std::string receive_timestamp;
+  };
+
+  std::vector<cv::Mat> images;
+  std::vector<std::string> current_timestamps;
+  std::vector<std::string> receive_timestamps;
+  std::vector<FrameMetadata> frame_metadata;
+  std::map<int64_t, PendingImage> pending_images;
+  std::map<int64_t, FrameMetadata> pending_metadata;
+  bool metadata_required = false;
+
+  void clear() {
+    images.clear();
+    current_timestamps.clear();
+    receive_timestamps.clear();
+    frame_metadata.clear();
+    pending_images.clear();
+    pending_metadata.clear();
+  }
 };
 
 class MultiCameraSubscriber : public rclcpp::Node {
  public:
   explicit MultiCameraSubscriber(const rclcpp::NodeOptions &options)
       : Node("MultiCameraSubscriber", options) {
-    device_init();
-  }
-  ~MultiCameraSubscriber() {
-    ir_image_buffers_.clear();
-    ir_current_timestamp_buffers_.clear();
-    ir_timestamp_buffers_.clear();
-    color_image_buffers_.clear();
-    color_current_timestamp_buffers_.clear();
-    color_timestamp_buffers_.clear();
-    left_ir_metadata_.exposure_buffs.clear();
-    left_ir_metadata_.gain_buffs.clear();
-    color_metadata_.exposure_buffs.clear();
-    color_metadata_.gain_buffs.clear();
-  }
-  void device_init() {
-    try {
-      auto context = std::make_unique<ob::Context>();
-      context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
-      auto list = context->queryDeviceList();
-      for (size_t i = 0; i < list->deviceCount(); i++) {
-        auto device = list->getDevice(i);
-        auto device_info = device->getDeviceInfo();
-        auto pid = device_info->getPid();
-        std::string serial = device_info->serialNumber();
-        std::string uid = device_info->uid();
-        auto usb_port = parseUsbPort(uid);
-        serial_numbers_[usb_port] = serial;
-        is_gemini330_ = isGemini330SeriesPID(pid);
-      }
-    } catch (ob::Error &e) {
-      RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e));
-    } catch (const std::exception &e) {
-      RCLCPP_ERROR_STREAM(get_logger(), e.what());
-    } catch (...) {
-      RCLCPP_ERROR_STREAM(get_logger(), "unknown error");
+    initializeDeviceInfo();
+    loadParameters();
+    for (size_t i = 0; i < usb_ports_.size(); ++i) {
+      usb_index_map_[usb_ports_[i]] = static_cast<int>(i);
     }
-    params_init();
-    for (size_t i = 0; i < usb_params_.size(); i++) {
-      usb_numbers_[i] = usb_params_[i];
-      usb_index_map_[usb_params_[i]] = i;
-    }
-    for (const auto &pair : serial_numbers_) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "usb_port: " << pair.first << ", serial: " << pair.second);
-    }
-    for (const auto &pair : usb_index_map_) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "usb_port: " << pair.first << ", index: " << pair.second);
+    for (const auto &entry : serial_numbers_) {
+      RCLCPP_INFO(get_logger(), "usb_port: %s, serial: %s", entry.first.c_str(),
+                  entry.second.c_str());
     }
     capture_control_srv_ = this->create_service<orbbec_camera_msgs::srv::SetInt32>(
         "start_capture", std::bind(&MultiCameraSubscriber::controlCaptureCallback, this,
@@ -74,328 +116,462 @@ class MultiCameraSubscriber : public rclcpp::Node {
   }
 
  private:
-  std::mutex image_mutex_;
-  std::mutex meta_mutex_;
-  void params_init() {
+  void initializeDeviceInfo() {
+    try {
+      auto context = std::make_unique<ob::Context>();
+      context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
+      auto list = context->queryDeviceList();
+      for (size_t i = 0; i < list->deviceCount(); ++i) {
+        auto device_info = list->getDevice(i)->getDeviceInfo();
+        const auto usb_port = parseUsbPort(device_info->uid());
+        serial_numbers_[usb_port] = device_info->serialNumber();
+      }
+    } catch (const ob::Error &e) {
+      RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e));
+    } catch (const std::exception &e) {
+      RCLCPP_ERROR_STREAM(get_logger(), e.what());
+    } catch (...) {
+      RCLCPP_ERROR(get_logger(), "unknown error while querying devices");
+    }
+  }
+
+  void loadParameters() {
     std::ifstream file(
         "install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"
         "multi_save_rgbir_params.json");
     if (!file.is_open()) {
-      RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file.");
+      RCLCPP_ERROR(get_logger(), "Failed to open JSON file.");
       return;
     }
+
     nlohmann::json json_data;
     file >> json_data;
-    time_domain_ = json_data["save_rgbir_params"]["time_domain"].get<std::string>();
-    time_domain_ =
-        (time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown");
-    usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get<std::vector<std::string>>();
-    camera_name_ = json_data["save_rgbir_params"]["camera_name"].get<std::vector<std::string>>();
-    left_ir_topics_.resize(camera_name_.size());
-    left_ir_metadata_topic_.resize(camera_name_.size());
-    color_topics_.resize(camera_name_.size());
-    color_metadata_topic_.resize(camera_name_.size());
-    for (size_t i = 0; i < camera_name_.size(); ++i) {
-      left_ir_topics_[i] =
-          "/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/image_raw";
-      left_ir_metadata_topic_[i] =
-          "/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/metadata";
-      color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
-      color_metadata_topic_[i] = "/" + camera_name_[i] + "/color/metadata";
+    const auto &params = json_data["save_rgbir_params"];
+    const auto time_domain = params["time_domain"].get<std::string>();
+    time_domain_suffix_ =
+        time_domain == "device" ? "_d" : (time_domain == "global" ? "_g" : "_unknown");
+    usb_ports_ = params["usb_ports"].get<std::vector<std::string>>();
+    camera_names_ = params["camera_name"].get<std::vector<std::string>>();
+
+    if (params.contains("stream_names")) {
+      for (const auto &stream_name : params["stream_names"].get<std::vector<std::string>>()) {
+        if (!isSupportedStreamName(stream_name)) {
+          throw std::invalid_argument("Unsupported stream name in multi_save_rgbir config: " +
+                                      stream_name);
+        }
+        if (std::find(configured_stream_names_.begin(), configured_stream_names_.end(),
+                      stream_name) == configured_stream_names_.end()) {
+          configured_stream_names_.push_back(stream_name);
+        }
+      }
     }
   }
-  void topic_init() {
-    ir_image_buffers_.resize(left_ir_topics_.size());
-    color_image_buffers_.resize(left_ir_topics_.size());
-    ir_current_timestamp_buffers_.resize(left_ir_topics_.size());
-    color_current_timestamp_buffers_.resize(left_ir_topics_.size());
-    ir_timestamp_buffers_.resize(left_ir_topics_.size());
-    color_timestamp_buffers_.resize(left_ir_topics_.size());
-    left_ir_metadata_.exposure_buffs.resize(left_ir_topics_.size());
-    left_ir_metadata_.gain_buffs.resize(left_ir_topics_.size());
-    color_metadata_.exposure_buffs.resize(left_ir_topics_.size());
-    color_metadata_.gain_buffs.resize(left_ir_topics_.size());
-    callback_called_ = std::vector<bool>(left_ir_topics_.size(), false);
-    auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
-    rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                       "camera_name_.size(): " << camera_name_.size());
-    for (size_t i = 0; i < camera_name_.size(); ++i) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "left_ir_topic: " << left_ir_topics_[i]);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "left_ir_metadata_topic_: " << left_ir_metadata_topic_[i]);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "color_topic: " << color_topics_[i]);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "color_metadata_topic_: " << color_metadata_topic_[i]);
-      reentrant_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-      rclcpp::SubscriptionOptions ir_sub_options;
-      ir_sub_options.callback_group = reentrant_callback_group_;
 
-      rclcpp::SubscriptionOptions color_sub_options;
-      color_sub_options.callback_group = reentrant_callback_group_;
+  std::vector<StreamTopicInfo> discoverStreams(const std::string &camera_name) const {
+    std::vector<StreamTopicInfo> streams;
+    const auto names_and_types = this->get_topic_names_and_types();
+    const std::string prefix = cameraNamespace(camera_name) + "/";
+    for (const auto &stream_name : kSupportedStreamNames) {
+      const auto image_topic_it = names_and_types.find(prefix + stream_name + "/image_raw");
+      if (image_topic_it == names_and_types.end()) {
+        continue;
+      }
+      const auto &image_types = image_topic_it->second;
+      if (std::find(image_types.begin(), image_types.end(), "sensor_msgs/msg/Image") ==
+          image_types.end()) {
+        continue;
+      }
 
-      auto ir_sub = this->create_subscription<sensor_msgs::msg::Image>(
-          left_ir_topics_[i], custom_qos,
-          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
-            this->irCallback(msg, i);
-          },
-          ir_sub_options);
-
-      auto ir_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
-          left_ir_metadata_topic_[i], custom_qos,
-          [this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
-            this->ir_meta_Callback(msg, i);
-          });
-
-      auto color_sub = this->create_subscription<sensor_msgs::msg::Image>(
-          color_topics_[i], custom_qos,
-          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
-            this->colorCallback(msg, i);
-          },
-          color_sub_options);
-
-      auto color_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
-          color_metadata_topic_[i], custom_qos,
-          [this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
-            this->color_meta_Callback(msg, i);
-          });
-
-      ir_subscribers_.push_back(ir_sub);
-      ir_meta_subscribers_.push_back(ir_metadata_sub);
-      color_subscribers_.push_back(color_sub);
-      color_meta_subscribers_.push_back(color_metadata_sub);
+      const auto metadata_topic_it = names_and_types.find(prefix + stream_name + "/metadata");
+      const bool metadata_available =
+          metadata_topic_it != names_and_types.end() &&
+          std::find(metadata_topic_it->second.begin(), metadata_topic_it->second.end(),
+                    "orbbec_camera_msgs/msg/Metadata") != metadata_topic_it->second.end();
+      streams.push_back(StreamTopicInfo{stream_name, metadata_available});
     }
+    return streams;
   }
-  std::string getCurrentTimes() {
-    auto now = std::chrono::system_clock::now();
-    auto now_time_t = std::chrono::system_clock::to_time_t(now);
-    std::tm tm = *std::localtime(&now_time_t);
-    std::ostringstream date_stream;
-    date_stream << std::put_time(&tm, "%Y%m%d%H%M%S");
 
-    std::string date_str = date_stream.str();
-    return date_str;
+  std::vector<std::vector<StreamTopicInfo>> waitForStableStreams() const {
+    if (!configured_stream_names_.empty()) {
+      std::vector<std::vector<StreamTopicInfo>> configured_streams;
+      configured_streams.reserve(camera_names_.size());
+      for (const auto &camera_name : camera_names_) {
+        const auto discovered_streams = discoverStreams(camera_name);
+        std::vector<StreamTopicInfo> camera_streams;
+        camera_streams.reserve(configured_stream_names_.size());
+        for (const auto &stream_name : configured_stream_names_) {
+          const auto discovered_it = std::find_if(
+              discovered_streams.begin(), discovered_streams.end(),
+              [&stream_name](const auto &stream) { return stream.name == stream_name; });
+          camera_streams.push_back(StreamTopicInfo{
+              stream_name,
+              discovered_it != discovered_streams.end() && discovered_it->metadata_available});
+        }
+        configured_streams.push_back(std::move(camera_streams));
+      }
+      return configured_streams;
+    }
+
+    std::vector<std::vector<StreamTopicInfo>> candidate;
+    auto candidate_since = std::chrono::steady_clock::time_point{};
+    const auto deadline = std::chrono::steady_clock::now() + kStreamDiscoveryTimeout;
+    while (rclcpp::ok() && std::chrono::steady_clock::now() < deadline) {
+      std::vector<std::vector<StreamTopicInfo>> current;
+      current.reserve(camera_names_.size());
+      bool all_cameras_discovered = !camera_names_.empty();
+      for (const auto &camera_name : camera_names_) {
+        current.push_back(discoverStreams(camera_name));
+        all_cameras_discovered = all_cameras_discovered && !current.back().empty();
+      }
+
+      const auto now = std::chrono::steady_clock::now();
+      if (!all_cameras_discovered) {
+        candidate.clear();
+      } else if (current != candidate) {
+        candidate = std::move(current);
+        candidate_since = now;
+      } else if (now - candidate_since >= kStreamDiscoveryStablePeriod) {
+        return candidate;
+      }
+
+      std::this_thread::sleep_for(kStreamDiscoveryPollInterval);
+    }
+
+    RCLCPP_WARN_STREAM(get_logger(),
+                       "Supported image topics did not become stable within "
+                           << kStreamDiscoveryTimeout.count()
+                           << " seconds; start all cameras and streams first, retry the request, "
+                              "or configure stream_names explicitly");
+    return {};
   }
-  std::string generateFolderName(const std::string &serial_number, size_t serial_index) {
-    std::string path = std::string("multicamera_sync/output/") + currenttimes_ + "/" +
-                       "TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" +
-                       std::to_string(serial_index);
 
+  bool initializeTopics() {
+    const auto streams_by_camera = waitForStableStreams();
+    if (streams_by_camera.size() != camera_names_.size()) {
+      return false;
+    }
+
+    callback_groups_.clear();
+    image_subscribers_.clear();
+    metadata_subscribers_.clear();
+    captures_.resize(camera_names_.size());
+    callback_called_.assign(camera_names_.size(), false);
+    const auto custom_qos =
+        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
+
+    for (size_t camera_index = 0; camera_index < camera_names_.size(); ++camera_index) {
+      const auto &streams = streams_by_camera[camera_index];
+      const std::string prefix = cameraNamespace(camera_names_[camera_index]) + "/";
+      auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+      callback_groups_.push_back(callback_group);
+      rclcpp::SubscriptionOptions options;
+      options.callback_group = callback_group;
+
+      for (const auto &stream : streams) {
+        const auto &stream_name = stream.name;
+        StreamCapture capture;
+        capture.metadata_required = stream.metadata_available;
+        captures_[camera_index].emplace(stream_name, std::move(capture));
+        const std::string image_topic = prefix + stream_name + "/image_raw";
+        RCLCPP_INFO(get_logger(), "Subscribing to %s", image_topic.c_str());
+
+        image_subscribers_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
+            image_topic, custom_qos,
+            [this, camera_index,
+             stream_name](const std::shared_ptr<const sensor_msgs::msg::Image> image) {
+              imageCallback(image, camera_index, stream_name);
+            },
+            options));
+        if (stream.metadata_available) {
+          const std::string metadata_topic = prefix + stream_name + "/metadata";
+          metadata_subscribers_.push_back(
+              this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
+                  metadata_topic, custom_qos,
+                  [this, camera_index, stream_name](
+                      const std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> metadata) {
+                    metadataCallback(metadata, camera_index, stream_name);
+                  },
+                  options));
+        }
+      }
+    }
+    return !captures_.empty() && std::all_of(captures_.begin(), captures_.end(),
+                                             [](const auto &streams) { return !streams.empty(); });
+  }
+
+  std::string currentDateTime() const {
+    const auto now = std::chrono::system_clock::now();
+    const auto now_time = std::chrono::system_clock::to_time_t(now);
+    const std::tm time_info = *std::localtime(&now_time);
+    std::ostringstream output;
+    output << std::put_time(&time_info, "%Y%m%d%H%M%S");
+    return output.str();
+  }
+
+  std::string generateFolderName(const std::string &serial_number, size_t serial_index) const {
+    const std::string path = "multicamera_sync/output/" + current_date_time_ +
+                             "/TotalModeFrames/SN" + serial_number + "_Index" +
+                             std::to_string(serial_index);
     std::filesystem::create_directories(path);
     return path;
   }
-  std::string getTimestamp() {
-    auto now = this->get_clock()->now();
-    int64_t seconds = now.seconds();
-    int64_t nanoseconds = now.nanoseconds() % 1000000000;
-    int64_t milliseconds = nanoseconds / 1000000;
+
+  std::string receiveTimestamp() const {
+    const auto now = this->get_clock()->now();
+    const int64_t seconds = now.seconds();
+    const int64_t milliseconds = now.nanoseconds() % 1000000000 / 1000000;
     return std::to_string(seconds) + std::to_string(milliseconds);
   }
-  std::string getCurrentTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image_msg) {
-    int64_t seconds = image_msg->header.stamp.sec;
-    int64_t nanoseconds = image_msg->header.stamp.nanosec;
-
-    int64_t milliseconds = nanoseconds / 1000000;
 
+  std::string imageTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image) const {
+    const int64_t milliseconds = image->header.stamp.nanosec / 1000000;
     std::ostringstream timestamp;
-    timestamp << seconds << std::setw(3) << std::setfill('0') << milliseconds;
-
+    timestamp << image->header.stamp.sec << std::setw(3) << std::setfill('0') << milliseconds;
     return timestamp.str();
   }
 
-  void saveAlignedImages(size_t index) {
-    auto &ir_images = ir_image_buffers_[index];
-    auto &ir_current_timestamps = ir_current_timestamp_buffers_[index];
-    auto &ir_timestamps = ir_timestamp_buffers_[index];
-    auto &color_images = color_image_buffers_[index];
-    auto &color_current_timestamps = color_current_timestamp_buffers_[index];
-    auto &color_timestamps = color_timestamp_buffers_[index];
-    auto &left_ir_meta_exposure = left_ir_metadata_.exposure_buffs[index];
-    auto &left_ir_meta_gain = left_ir_metadata_.gain_buffs[index];
-    auto &color_meta_exposure = color_metadata_.exposure_buffs[index];
-    auto &color_meta_gain = color_metadata_.gain_buffs[index];
-    callback_called_[index] = true;
-    if (ir_images.size() < static_cast<size_t>(saving_images_number_) ||
-        color_images.size() < static_cast<size_t>(saving_images_number_)) {
+  bool captureReady(size_t camera_index) const {
+    if (camera_index >= captures_.size() || captures_[camera_index].empty()) {
+      return false;
+    }
+    return std::all_of(
+        captures_[camera_index].begin(), captures_[camera_index].end(), [this](const auto &entry) {
+          return entry.second.images.size() >= static_cast<size_t>(saving_images_number_);
+        });
+  }
+
+  int64_t messageStampNs(const std_msgs::msg::Header &header) const {
+    return static_cast<int64_t>(header.stamp.sec) * 1000000000LL + header.stamp.nanosec;
+  }
+
+  size_t pendingFrameLimit() const {
+    return std::max(kMinimumPendingFrameLimit, static_cast<size_t>(saving_images_number_) * 2);
+  }
+
+  template <typename Value>
+  void trimPendingFrames(std::map<int64_t, Value> &pending) const {
+    while (pending.size() > pendingFrameLimit()) {
+      pending.erase(pending.begin());
+    }
+  }
+
+  void appendCompletedFrame(StreamCapture &capture, StreamCapture::PendingImage pending_image,
+                            StreamCapture::FrameMetadata metadata = {}) {
+    if (capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
       return;
     }
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "index:" << index);
-    auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
-    auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
-    int usb_index = usb_iter->second;
-    if (serial_iter == serial_numbers_.end()) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "serial_iter is empty");
+    capture.images.push_back(std::move(pending_image.image));
+    capture.current_timestamps.push_back(std::move(pending_image.current_timestamp));
+    capture.receive_timestamps.push_back(std::move(pending_image.receive_timestamp));
+    capture.frame_metadata.push_back(std::move(metadata));
+  }
+
+  std::string metadataSuffix(const StreamCapture &capture, size_t frame_index) const {
+    if (frame_index >= capture.frame_metadata.size()) {
+      return "";
+    }
+    std::string suffix;
+    if (!capture.frame_metadata[frame_index].exposure.empty()) {
+      suffix += "_e" + capture.frame_metadata[frame_index].exposure;
+    }
+    if (!capture.frame_metadata[frame_index].gain.empty()) {
+      suffix += "_d" + capture.frame_metadata[frame_index].gain;
+    }
+    return suffix;
+  }
+
+  void saveImages(size_t camera_index) {
+    if (!captureReady(camera_index)) {
       return;
     }
-    std::string serial_index = serial_iter->second;
+    if (camera_index >= usb_ports_.size()) {
+      RCLCPP_ERROR(get_logger(), "Missing USB port configuration for camera index %zu",
+                   camera_index);
+      return;
+    }
+    const auto serial_it = serial_numbers_.find(usb_ports_[camera_index]);
+    if (serial_it == serial_numbers_.end()) {
+      RCLCPP_ERROR(get_logger(), "No serial number found for USB port %s",
+                   usb_ports_[camera_index].c_str());
+      return;
+    }
+    const auto usb_index_it = usb_index_map_.find(usb_ports_[camera_index]);
+    const size_t usb_index = usb_index_it == usb_index_map_.end()
+                                 ? camera_index
+                                 : static_cast<size_t>(usb_index_it->second);
+    const std::string &serial_number = serial_it->second;
+    const std::string folder = generateFolderName(serial_number, usb_index);
+    callback_called_[camera_index] = true;
 
-    for (size_t i = 0; i < static_cast<size_t>(saving_images_number_); i++) {
-      std::string folder = generateFolderName(serial_index, usb_index);
-      std::string ir_filename =
-          folder + "/ir#left_SN" + serial_index + "_Index" + std::to_string(usb_index) +
-          time_domain_ + ir_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
-          ir_timestamps[i] +
-          (is_gemini330_ ? ("_e" + left_ir_meta_exposure[i] + "_d" + left_ir_meta_gain[i]) : "") +
-          "_.jpg";
-      if (ir_images[i].empty()) {
-        RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
-        continue;
+    for (const auto &entry : captures_[camera_index]) {
+      const std::string &stream_name = entry.first;
+      const auto &capture = entry.second;
+      for (size_t i = 0; i < static_cast<size_t>(saving_images_number_); ++i) {
+        if (capture.images[i].empty()) {
+          continue;
+        }
+        const std::string filename = folder + "/" + stream_name + "_SN" + serial_number + "_Index" +
+                                     std::to_string(usb_index) + time_domain_suffix_ +
+                                     capture.current_timestamps[i] + "_f" + std::to_string(i) +
+                                     "_s" + capture.receive_timestamps[i] +
+                                     metadataSuffix(capture, i) + "_.jpg";
+        cv::imwrite(filename, capture.images[i]);
       }
-
-      cv::imwrite(ir_filename, ir_images[i]);
-      std::string color_filename =
-          folder + "/color_SN" + serial_index + "_Index" + std::to_string(usb_index) +
-          time_domain_ + color_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
-          color_timestamps[i] +
-          (is_gemini330_ ? ("_e" + color_meta_exposure[i] + "_d" + color_meta_gain[i]) : "") +
-          "_.jpg";
-      if (color_images[i].empty()) {
-        continue;
-      }
-      cv::imwrite(color_filename, color_images[i]);
-      // RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str());
     }
 
-    ir_image_buffers_[index].clear();
-    ir_current_timestamp_buffers_[index].clear();
-    ir_timestamp_buffers_[index].clear();
-    color_image_buffers_[index].clear();
-    color_current_timestamp_buffers_[index].clear();
-    color_timestamp_buffers_[index].clear();
-    left_ir_metadata_.exposure_buffs[index].clear();
-    left_ir_metadata_.gain_buffs[index].clear();
-    color_metadata_.exposure_buffs[index].clear();
-    color_metadata_.gain_buffs[index].clear();
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                       "callback_called_ " << index << ":" << callback_called_[index]);
-    bool all_true =
-        std::all_of(callback_called_.begin(), callback_called_.end(), [](bool v) { return v; });
-    if (all_true) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
+    for (auto &entry : captures_[camera_index]) {
+      entry.second.clear();
+    }
+    const bool all_cameras_complete = std::all_of(callback_called_.begin(), callback_called_.end(),
+                                                  [](bool value) { return value; });
+    if (all_cameras_complete) {
+      RCLCPP_INFO(get_logger(), "Capture completed for all cameras");
       saving_images_number_ = 0;
-      callback_called_.clear();
-      callback_called_ = std::vector<bool>(left_ir_topics_.size(), false);
+      callback_called_.assign(camera_names_.size(), false);
     }
   }
 
   void controlCaptureCallback(
       const std::shared_ptr<orbbec_camera_msgs::srv::SetInt32::Request> request,
       std::shared_ptr<orbbec_camera_msgs::srv::SetInt32::Response> response) {
-    (void)response;
-    currenttimes_ = getCurrentTimes();
+    std::lock_guard<std::mutex> lock(capture_mutex_);
+    if (request->data <= 0) {
+      response->success = false;
+      response->message = "capture image count must be greater than zero";
+      return;
+    }
+    if (!topics_initialized_) {
+      if (!initializeTopics()) {
+        callback_groups_.clear();
+        image_subscribers_.clear();
+        metadata_subscribers_.clear();
+        captures_.clear();
+        callback_called_.clear();
+        response->success = false;
+        response->message =
+            "supported image streams did not become stable; retry after all cameras and streams "
+            "start or configure stream_names";
+        return;
+      }
+      topics_initialized_ = true;
+    }
+
+    for (auto &camera_captures : captures_) {
+      for (auto &entry : camera_captures) {
+        entry.second.clear();
+      }
+    }
+    callback_called_.assign(camera_names_.size(), false);
+    current_date_time_ = currentDateTime();
     saving_images_number_ = request->data;
-    if (!topic_init_) {
-      topic_init();
-      topic_init_ = true;
-    }
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                       "saving_images_number_: " << saving_images_number_);
-  }
-  void irCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
-    std::lock_guard<std::mutex> lock(image_mutex_);
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
-      cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
-      std::string current_timestamp_ir = getCurrentTimestamp(image);
-      std::string timestamp_ir = getTimestamp();
-      ir_image_buffers_[index].push_back(ir_mat);
-      ir_current_timestamp_buffers_[index].push_back(current_timestamp_ir);
-      ir_timestamp_buffers_[index].push_back(timestamp_ir);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         ":ir: " << index << ":" << ir_image_buffers_[index].size());
-      if (ir_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
-          color_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
-          (!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >=
-                                  static_cast<size_t>(saving_images_number_) &&
-                              left_ir_metadata_.exposure_buffs[index].size() >=
-                                  static_cast<size_t>(saving_images_number_)))) {
-        saveAlignedImages(index);
-      }
-    }
-  }
-  void colorCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
-    std::lock_guard<std::mutex> lock(image_mutex_);
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
-      cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
-      cv::Mat corrected_image;
-      cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR);
-      std::string current_timestamp_color = getCurrentTimestamp(image);
-      std::string timestamp_color = getTimestamp();
-      color_image_buffers_[index].push_back(corrected_image);
-      color_current_timestamp_buffers_[index].push_back(current_timestamp_color);
-      color_timestamp_buffers_[index].push_back(timestamp_color);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         ":color: " << index << ":" << color_image_buffers_[index].size());
-      if (ir_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
-          color_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
-          (!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >=
-                                  static_cast<size_t>(saving_images_number_) &&
-                              left_ir_metadata_.exposure_buffs[index].size() >=
-                                  static_cast<size_t>(saving_images_number_)))) {
-        saveAlignedImages(index);
-      }
-    }
+    response->success = true;
+    response->message = "capture started";
+    RCLCPP_INFO(get_logger(), "Capturing %d image(s) from each configured stream",
+                saving_images_number_);
   }
 
-  void ir_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
-                        size_t index) {
-    std::lock_guard<std::mutex> lock(meta_mutex_);
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
-      nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
-      left_ir_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
-      left_ir_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
+  void imageCallback(const std::shared_ptr<const sensor_msgs::msg::Image> image,
+                     size_t camera_index, const std::string &stream_name) {
+    std::lock_guard<std::mutex> lock(capture_mutex_);
+    if (saving_images_number_ <= 0 || callback_called_[camera_index]) {
+      return;
     }
-  }
-  void color_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
-                           size_t index) {
-    std::lock_guard<std::mutex> lock(meta_mutex_);
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
-      nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
-      color_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
-      color_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
+    auto &capture = captures_[camera_index].at(stream_name);
+    if (capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
+      saveImages(camera_index);
+      return;
     }
+    cv::Mat output = cv_bridge::toCvCopy(image, image->encoding)->image;
+    if (isColorCaptureStreamName(stream_name) &&
+        image->encoding == sensor_msgs::image_encodings::RGB8) {
+      cv::Mat converted;
+      cv::cvtColor(output, converted, cv::COLOR_RGB2BGR);
+      output = converted;
+    } else if (isColorCaptureStreamName(stream_name) &&
+               image->encoding == sensor_msgs::image_encodings::RGBA8) {
+      cv::Mat converted;
+      cv::cvtColor(output, converted, cv::COLOR_RGBA2BGRA);
+      output = converted;
+    }
+    StreamCapture::PendingImage pending_image{std::move(output), imageTimestamp(image),
+                                              receiveTimestamp()};
+    if (capture.metadata_required) {
+      const auto stamp_ns = messageStampNs(image->header);
+      auto metadata_it = capture.pending_metadata.find(stamp_ns);
+      if (metadata_it == capture.pending_metadata.end()) {
+        capture.pending_images.insert_or_assign(stamp_ns, std::move(pending_image));
+        trimPendingFrames(capture.pending_images);
+        return;
+      }
+      appendCompletedFrame(capture, std::move(pending_image), std::move(metadata_it->second));
+      capture.pending_metadata.erase(metadata_it);
+    } else {
+      appendCompletedFrame(capture, std::move(pending_image));
+    }
+    RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index,
+                capture.images.size(), saving_images_number_);
+    saveImages(camera_index);
   }
 
+  void metadataCallback(const std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> metadata,
+                        size_t camera_index, const std::string &stream_name) {
+    std::lock_guard<std::mutex> lock(capture_mutex_);
+    if (saving_images_number_ <= 0 || callback_called_[camera_index]) {
+      return;
+    }
+    auto &capture = captures_[camera_index].at(stream_name);
+    if (!capture.metadata_required ||
+        capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
+      return;
+    }
+    StreamCapture::FrameMetadata frame_metadata;
+    try {
+      const auto json_data = nlohmann::json::parse(metadata->json_data);
+      if (json_data.contains("exposure")) {
+        frame_metadata.exposure = json_data["exposure"].dump();
+      }
+      if (json_data.contains("gain")) {
+        frame_metadata.gain = json_data["gain"].dump();
+      }
+    } catch (const std::exception &e) {
+      RCLCPP_WARN(get_logger(), "Failed to parse %s metadata: %s", stream_name.c_str(), e.what());
+    }
+    const auto stamp_ns = messageStampNs(metadata->header);
+    auto image_it = capture.pending_images.find(stamp_ns);
+    if (image_it == capture.pending_images.end()) {
+      capture.pending_metadata.insert_or_assign(stamp_ns, std::move(frame_metadata));
+      trimPendingFrames(capture.pending_metadata);
+      return;
+    }
+    appendCompletedFrame(capture, std::move(image_it->second), std::move(frame_metadata));
+    capture.pending_images.erase(image_it);
+    RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index,
+                capture.images.size(), saving_images_number_);
+    saveImages(camera_index);
+  }
+
+  std::mutex capture_mutex_;
+  std::vector<rclcpp::CallbackGroup::SharedPtr> callback_groups_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> image_subscribers_;
   std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
-      ir_meta_subscribers_;
-  std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
-      color_meta_subscribers_;
-  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> ir_subscribers_;
-  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subscribers_;
+      metadata_subscribers_;
   rclcpp::Service<orbbec_camera_msgs::srv::SetInt32>::SharedPtr capture_control_srv_;
 
   std::map<std::string, int> usb_index_map_;
   std::map<std::string, std::string> serial_numbers_;
-  std::array<std::string, 10> usb_numbers_;
-
-  std::vector<std::string> usb_params_;
-  std::vector<std::string> camera_name_;
-  std::vector<std::string> left_ir_metadata_topic_;
-  std::vector<std::string> color_metadata_topic_;
-  std::vector<std::string> left_ir_topics_;
-  std::vector<std::string> color_topics_;
-  std::string time_domain_;
-
-  std::vector<std::vector<cv::Mat>> ir_image_buffers_;
-  std::vector<std::vector<cv::Mat>> color_image_buffers_;
-  std::vector<std::vector<std::string>> ir_current_timestamp_buffers_;
-  std::vector<std::vector<std::string>> color_current_timestamp_buffers_;
-  std::vector<std::vector<std::string>> ir_timestamp_buffers_;
-  std::vector<std::vector<std::string>> color_timestamp_buffers_;
-
+  std::vector<std::string> usb_ports_;
+  std::vector<std::string> camera_names_;
+  std::vector<std::string> configured_stream_names_;
+  std::vector<std::map<std::string, StreamCapture>> captures_;
   std::vector<bool> callback_called_;
-
-  std::string currenttimes_;
-
-  int saving_images_number_ = 100;
-
-  bool topic_init_ = false;
-  bool is_gemini330_ = true;
-
-  ImageMetadata left_ir_metadata_ = ImageMetadata();
-  ImageMetadata color_metadata_ = ImageMetadata();
+  std::string time_domain_suffix_;
+  std::string current_date_time_;
+  int saving_images_number_ = 0;
+  bool topics_initialized_ = false;
 };
+
 }  // namespace tools
 }  // namespace orbbec_camera
+
 RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::tools::MultiCameraSubscriber)

orbbec_camera/tools/start_benchmark.cpp

@@ -19,6 +19,16 @@ class StartBenchmark : public rclcpp::Node {
           [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
             this->color_Callback(msg, i);
           }));
+      left_color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
+          left_color_topics_[i], custom_qos,
+          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+            this->leftColorCallback(msg, i);
+          }));
+      right_color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
+          right_color_topics_[i], custom_qos,
+          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+            this->rightColorCallback(msg, i);
+          }));
       depth_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
           depth_topics_[i], custom_qos,
           [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
@@ -45,6 +55,10 @@ class StartBenchmark : public rclcpp::Node {
             this->color_point_cloud_Callback(msg, i);
           }));
       RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), color_topics_[i] << " is subed ");
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
+                         left_color_topics_[i] << " is subed ");
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
+                         right_color_topics_[i] << " is subed ");
       RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), depth_topics_[i] << " is subed ");
       RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), left_ir_topics_[i] << " is subed ");
       RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), right_ir_topics_[i] << " is subed ");
@@ -57,6 +71,8 @@ class StartBenchmark : public rclcpp::Node {
 
  private:
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subs_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> left_color_subs_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> right_color_subs_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> depth_subs_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> left_ir_subs_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> right_ir_subs_;
@@ -67,6 +83,8 @@ class StartBenchmark : public rclcpp::Node {
 
   std::vector<std::string> camera_name_;
   std::vector<std::string> color_topics_;
+  std::vector<std::string> left_color_topics_;
+  std::vector<std::string> right_color_topics_;
   std::vector<std::string> depth_topics_;
   std::vector<std::string> left_ir_topics_;
   std::vector<std::string> right_ir_topics_;
@@ -88,6 +106,8 @@ class StartBenchmark : public rclcpp::Node {
     camera_name_ =
         json_data["start_benchmark_params"]["camera_name"].get<std::vector<std::string>>();
     color_topics_.resize(camera_name_.size());
+    left_color_topics_.resize(camera_name_.size());
+    right_color_topics_.resize(camera_name_.size());
     depth_topics_.resize(camera_name_.size());
     left_ir_topics_.resize(camera_name_.size());
     right_ir_topics_.resize(camera_name_.size());
@@ -95,6 +115,8 @@ class StartBenchmark : public rclcpp::Node {
     color_point_cloud_topics_.resize(camera_name_.size());
     for (size_t i = 0; i < camera_name_.size(); ++i) {
       color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
+      left_color_topics_[i] = "/" + camera_name_[i] + "/left_color/image_raw";
+      right_color_topics_[i] = "/" + camera_name_[i] + "/right_color/image_raw";
       depth_topics_[i] = "/" + camera_name_[i] + "/depth/image_raw";
       left_ir_topics_[i] = "/" + camera_name_[i] + "/left_ir/image_raw";
       right_ir_topics_[i] = "/" + camera_name_[i] + "/right_ir/image_raw";
@@ -108,6 +130,17 @@ class StartBenchmark : public rclcpp::Node {
     RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
                         "time is : " << msg->step << "color is subed " << index << "is subed");
   }
+  void leftColorCallback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
+                        "time is : " << msg->step << "left_color is subed " << index << "is subed");
+  }
+  void rightColorCallback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    RCLCPP_DEBUG_STREAM(
+        rclcpp::get_logger("StartBenchmark"),
+        "time is : " << msg->step << "right_color is subed " << index << "is subed");
+  }
   void depth_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
     std::lock_guard<std::mutex> lock(image_mutex_);
     RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),