fix: support side streams in camera tools

orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json

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

orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp

@@ -33,6 +33,7 @@
 #include <opencv2/opencv.hpp>
 #include <string>
 #include <thread>
+#include <utility>
 #include <vector>
 
 using Image = sensor_msgs::msg::Image;
@@ -69,7 +70,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 +167,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 +199,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 +226,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 "
+          "No image topics to synchronize. Set parameter sync_topics or start supported camera "
-          "before this node.");
+          "streams before this node.");
     }
 
     std::vector<std::string> deduplicated_topics;
@@ -233,7 +252,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 +266,13 @@ class ImageSyncNode : public rclcpp::Node {
       info.image_type = "image";
       info.camera_name = topic;
 
-      const auto color_pos = topic.rfind("/color/image_raw");
+      for (const auto &entry : supported_stream_suffixes()) {
-      const auto depth_pos = topic.rfind("/depth/image_raw");
+        const std::string suffix = entry.second;
-      if (color_pos != std::string::npos) {
+        if (has_suffix(topic, suffix)) {
-        info.image_type = "color";
+          info.image_type = entry.first;
-        info.camera_name = topic.substr(0, color_pos);
+          info.camera_name = topic.substr(0, topic.size() - suffix.size());
-      } else if (depth_pos != std::string::npos) {
+          break;
-        info.image_type = "depth";
+        }
-        info.camera_name = topic.substr(0, depth_pos);
       }
 
       const auto slash_pos = info.camera_name.find_last_of('/');
@@ -550,10 +568,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"
+    std::cout << "cur: " << cur << " ms" << " avg: " << avg_diff << " ms" << " max: " << max_diff_
-              << " avg: " << avg_diff << " ms"
+              << " ms" << " min: " << min_diff_ << " ms" << std::endl;
-              << " max: " << max_diff_ << " ms"
-              << " min: " << min_diff_ << " ms" << std::endl;
 
     if (last_time_ == 0.0) {
       last_time_ = base_t;

orbbec_camera/scripts/common_benchmark_node.py

@@ -26,6 +26,14 @@ from sensor_msgs.msg import Image
 from tabulate import tabulate
 
 CAMERA_NODE_NAMES = ["component_container", "orbbec_camera_node", "nodelet"]
+MONITORED_STREAMS = (
+    "color",
+    "depth",
+    "left_ir",
+    "right_ir",
+    "left_color",
+    "right_color",
+)
 DOCUMENTATION_URL = (
     "https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/"
     "6_benchmark/benchmark_tools.html"
@@ -110,13 +118,18 @@ class TopicTracker:
     def on_msg(self, header, avg_fps):
         stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
         self.received += 1
+        observed_fps = None
 
-        if self.last_time is not None and avg_fps > 0:
+        if self.last_time is not None:
             dt = stamp - self.last_time
-            expected_interval = 1.0 / avg_fps
+            if dt > 0:
-            self.drop_frames += estimate_dropped_frames(dt, expected_interval)
+                observed_fps = 1.0 / dt
+                if avg_fps > 0:
+                    expected_interval = 1.0 / avg_fps
+                    self.drop_frames += estimate_dropped_frames(dt, expected_interval)
 
         self.last_time = stamp
+        return stamp, observed_fps
 
     def frames_loss_rate(self):
         total = self.received + self.drop_frames
@@ -154,8 +167,8 @@ class CameraMonitorNode(Node):
                 "cpu_stats": make_stat(),
                 "ram_stats": make_stat(),
                 "trackers": {
-                    "color": TopicTracker(logger=self.get_logger()),
+                    stream: TopicTracker(logger=self.get_logger())
-                    "depth": TopicTracker(logger=self.get_logger())
+                    for stream in MONITORED_STREAMS
                 }
             }
 
@@ -177,18 +190,15 @@ class CameraMonitorNode(Node):
                 lambda msg, name=camera_name: self.status_callback(msg, name),
                 5
             )
-            self.create_subscription(
+            for stream in MONITORED_STREAMS:
-                Image,
+                self.create_subscription(
-                f"{ns}/color/image_raw",
+                    Image,
-                lambda msg, name=camera_name: self.image_callback(msg, name, "color"),
+                    f"{ns}/{stream}/image_raw",
-                5
+                    lambda msg, name=camera_name, stream_name=stream: self.image_callback(
-            )
+                        msg, name, stream_name
-            self.create_subscription(
+                    ),
-                Image,
+                    5,
-                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)
@@ -318,14 +328,27 @@ class CameraMonitorNode(Node):
         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"):
+        if stream not in MONITORED_STREAMS:
             return
-        header = msg.header
         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"]
+        fps_to_use = (
-        tracker.on_msg(header, fps_to_use)
+            self.ideal_fps
+            if self.ideal_fps and self.ideal_fps > 0.0
+            else camera["stats"][f"{stream}_fps"]["avg"]
+        )
+        stamp, observed_fps = tracker.on_msg(msg.header, fps_to_use)
+
+        # DeviceStatus currently reports detailed values only for the main color/depth streams.
+        # Derive equivalent statistics from image timestamps for the side streams.
+        if stream not in ("color", "depth"):
+            if observed_fps is not None:
+                self.update_sample_stat(camera["stats"], f"{stream}_fps", observed_fps)
+            delay_ms = (self.get_clock().now().nanoseconds * 1e-9 - stamp) * 1000.0
+            # Device-domain timestamps are not comparable with the ROS clock.
+            if 0.0 <= delay_ms <= 60000.0:
+                self.update_sample_stat(camera["stats"], f"{stream}_delay", delay_ms)
 
     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
@@ -338,6 +361,17 @@ class CameraMonitorNode(Node):
         s["min"] = min(s["min"], min_val)
         s["max"] = max(s["max"], max_val)
 
+    def update_sample_stat(self, stats, key, value):
+        if value is None or value < 0.0:
+            return
+        s = stats[key]
+        s["cur"] = value
+        s["count"] += 1
+        s["sum"] += value
+        s["avg"] = s["sum"] / s["count"]
+        s["min"] = min(s["min"], value)
+        s["max"] = max(s["max"], value)
+
     def update_sys_stat(self, stat_dict, value, online=True):
         stat_dict["cur"] = value
         if value is None or value <= 0.0 or not online:
@@ -365,17 +399,28 @@ class CameraMonitorNode(Node):
 
     def build_csv_header(self):
         header = ["time(s)"]
-        camera_fields = [
+        camera_fields = ["connection_type", "status_online", "disconnects"]
-            "connection_type", "status_online", "disconnects",
+        for stream in MONITORED_STREAMS:
-            "color_fps_cur", "color_fps_avg", "color_fps_min", "color_fps_max",
+            camera_fields.extend(
-            "color_delay_cur", "color_delay_avg", "color_delay_min", "color_delay_max",
+                [
-            "depth_fps_cur", "depth_fps_avg", "depth_fps_min", "depth_fps_max",
+                    f"{stream}_fps_cur",
-            "depth_delay_cur", "depth_delay_avg", "depth_delay_min", "depth_delay_max",
+                    f"{stream}_fps_avg",
-            "cpu_cur", "cpu_avg", "cpu_min", "cpu_max",
+                    f"{stream}_fps_min",
-            "ram_cur", "ram_avg", "ram_min", "ram_max",
+                    f"{stream}_fps_max",
-            "color_frames_loss", "color_frames_loss_rate(%)",
+                    f"{stream}_delay_cur",
-            "depth_frames_loss", "depth_frames_loss_rate(%)"
+                    f"{stream}_delay_avg",
-        ]
+                    f"{stream}_delay_min",
+                    f"{stream}_delay_max",
+                    f"{stream}_frames_loss",
+                    f"{stream}_frames_loss_rate(%)",
+                ]
+            )
+        camera_fields.extend(
+            [
+                "cpu_cur", "cpu_avg", "cpu_min", "cpu_max",
+                "ram_cur", "ram_avg", "ram_min", "ram_max",
+            ]
+        )
         for camera_name in self.camera_names:
             header.extend([f"{camera_name}_{field}" for field in camera_fields])
 
@@ -386,9 +431,6 @@ class CameraMonitorNode(Node):
         return header
 
     def build_camera_csv_values(self, camera):
-        color_tracker = camera["trackers"]["color"]
-        depth_tracker = camera["trackers"]["depth"]
-
         def safe(k):
             v = camera["stats"].get(k, {})
             return (
@@ -398,29 +440,46 @@ class CameraMonitorNode(Node):
                 self.format_csv_number(v.get("max", 0.0)),
             )
 
-        if not camera["prev_online"]:
+        values = [
-            return [
+            camera["connection_type"],
-                camera["connection_type"], camera["prev_online"], camera["disconnect_count"],
+            camera["prev_online"],
-                *["N/A"] * 16,
+            camera["disconnect_count"],
-                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)
         ]
+        for stream in MONITORED_STREAMS:
+            tracker = camera["trackers"][stream]
+            if camera["prev_online"]:
+                values.extend(safe(f"{stream}_fps"))
+                values.extend(safe(f"{stream}_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,7 +495,7 @@ class CameraMonitorNode(Node):
         rows = []
         for camera_name in self.camera_names:
             camera = self.cameras[camera_name]
-            for stream in ["color", "depth"]:
+            for stream in MONITORED_STREAMS:
                 fps_key = f"{stream}_fps"
                 delay_key = f"{stream}_delay"
                 topic_name = f"/{camera_name}/{stream}/image_raw"

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 ||
+      } else if (sensor->getType() == OB_SENSOR_COLOR ||
-                 sensor->getType() == OB_SENSOR_IR || sensor->getType() == OB_SENSOR_IR_LEFT ||
+                 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,84 @@
-
 #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 <algorithm>
-#include "orbbec_camera/ob_camera_node.h"
+#include <array>
-#include "orbbec_camera_msgs/msg/metadata.hpp"
+#include <chrono>
-#include <std_msgs/msg/int32.hpp>
+#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 <vector>
+
+#include <nlohmann/json.hpp>
+#include <sensor_msgs/image_encodings.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 {
+namespace {
-  std::vector<std::vector<std::string>> exposure_buffs;
+
-  std::vector<std::vector<std::string>> gain_buffs;
+const std::array<std::string, 6> kSupportedStreamNames = {
+    "color", "left_color", "right_color", "ir", "left_ir", "right_ir",
+};
+
+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 {
+  std::vector<cv::Mat> images;
+  std::vector<std::string> current_timestamps;
+  std::vector<std::string> receive_timestamps;
+  std::vector<std::string> exposures;
+  std::vector<std::string> gains;
+
+  void clear() {
+    images.clear();
+    current_timestamps.clear();
+    receive_timestamps.clear();
+    exposures.clear();
+    gains.clear();
+  }
 };
 
 class MultiCameraSubscriber : public rclcpp::Node {
  public:
   explicit MultiCameraSubscriber(const rclcpp::NodeOptions &options)
       : Node("MultiCameraSubscriber", options) {
-    device_init();
+    initializeDeviceInfo();
-  }
+    loadParameters();
-  ~MultiCameraSubscriber() {
+    for (size_t i = 0; i < usb_ports_.size(); ++i) {
-    ir_image_buffers_.clear();
+      usb_index_map_[usb_ports_[i]] = static_cast<int>(i);
-    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_ = isGemini335PID(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");
     }
-    params_init();
+    for (const auto &entry : serial_numbers_) {
-    for (size_t i = 0; i < usb_params_.size(); i++) {
+      RCLCPP_INFO(get_logger(), "usb_port: %s, serial: %s", entry.first.c_str(),
-      usb_numbers_[i] = usb_params_[i];
+                  entry.second.c_str());
-      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);
     }
     capture_control_srv_ = this->create_service<orbbec_camera_msgs::srv::SetInt32>(
         "start_capture", std::bind(&MultiCameraSubscriber::controlCaptureCallback, this,
@@ -74,9 +86,27 @@ class MultiCameraSubscriber : public rclcpp::Node {
   }
 
  private:
-  std::mutex image_mutex_;
+  void initializeDeviceInfo() {
-  std::mutex meta_mutex_;
+    try {
-  bool isGemini335PID(uint32_t pid) {
+      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();
+        has_gemini330_device_ = has_gemini330_device_ || isGemini330Series(device_info->getPid());
+      }
+    } 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");
+    }
+  }
+
+  bool isGemini330Series(uint32_t pid) const {
     return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
            pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
            pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
@@ -85,326 +115,312 @@ class MultiCameraSubscriber : public rclcpp::Node {
            pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
            pid == GEMINI_331L_PID;
   }
-  void params_init() {
+
+  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>();
+    const auto &params = json_data["save_rgbir_params"];
-    time_domain_ =
+    const auto time_domain = params["time_domain"].get<std::string>();
-        (time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown");
+    time_domain_suffix_ =
-    usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get<std::vector<std::string>>();
+        time_domain == "device" ? "_d" : (time_domain == "global" ? "_g" : "_unknown");
-    camera_name_ = json_data["save_rgbir_params"]["camera_name"].get<std::vector<std::string>>();
+    usb_ports_ = params["usb_ports"].get<std::vector<std::string>>();
-    left_ir_topics_.resize(camera_name_.size());
+    camera_names_ = params["camera_name"].get<std::vector<std::string>>();
-    left_ir_metadata_topic_.resize(camera_name_.size());
+
-    color_topics_.resize(camera_name_.size());
+    if (params.contains("stream_names")) {
-    color_metadata_topic_.resize(camera_name_.size());
+      for (const auto &stream_name : params["stream_names"].get<std::vector<std::string>>()) {
-    for (size_t i = 0; i < camera_name_.size(); ++i) {
+        if (!isSupportedStreamName(stream_name)) {
-      left_ir_topics_[i] =
+          throw std::invalid_argument("Unsupported stream name in multi_save_rgbir config: " +
-          "/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/image_raw";
+                                      stream_name);
-      left_ir_metadata_topic_[i] =
+        }
-          "/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/metadata";
+        if (std::find(configured_stream_names_.begin(), configured_stream_names_.end(),
-      color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
+                      stream_name) == configured_stream_names_.end()) {
-      color_metadata_topic_[i] = "/" + camera_name_[i] + "/color/metadata";
+          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;
+  std::vector<std::string> discoverStreamNames(const std::string &camera_name) const {
-      color_sub_options.callback_group = reentrant_callback_group_;
+    std::vector<std::string> stream_names;
+    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 std::string topic = prefix + stream_name + "/image_raw";
+      const auto topic_it = names_and_types.find(topic);
+      if (topic_it == names_and_types.end()) {
+        continue;
+      }
+      const auto &types = topic_it->second;
+      if (std::find(types.begin(), types.end(), "sensor_msgs/msg/Image") != types.end()) {
+        stream_names.push_back(stream_name);
+      }
+    }
+    return stream_names;
+  }
 
-      auto ir_sub = this->create_subscription<sensor_msgs::msg::Image>(
+  std::vector<std::string> selectStreamNames(const std::string &camera_name) const {
-          left_ir_topics_[i], custom_qos,
+    if (!configured_stream_names_.empty()) {
-          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+      return configured_stream_names_;
-            this->irCallback(msg, i);
+    }
-          },
+    auto stream_names = discoverStreamNames(camera_name);
-          ir_sub_options);
+    if (!stream_names.empty()) {
+      return stream_names;
+    }
 
-      auto ir_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
+    RCLCPP_WARN(get_logger(),
-          left_ir_metadata_topic_[i], custom_qos,
+                "No supported image topics discovered for %s; using legacy RGB/IR topics",
-          [this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
+                camera_name.c_str());
-            this->ir_meta_Callback(msg, i);
+    return {has_gemini330_device_ ? "left_ir" : "ir", "color"};
-          });
+  }
 
-      auto color_sub = this->create_subscription<sensor_msgs::msg::Image>(
+  void initializeTopics() {
-          color_topics_[i], custom_qos,
+    captures_.resize(camera_names_.size());
-          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+    callback_called_.assign(camera_names_.size(), false);
-            this->colorCallback(msg, i);
+    const auto custom_qos =
-          },
+        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
-          color_sub_options);
 
-      auto color_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
+    for (size_t camera_index = 0; camera_index < camera_names_.size(); ++camera_index) {
-          color_metadata_topic_[i], custom_qos,
+      const auto stream_names = selectStreamNames(camera_names_[camera_index]);
-          [this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
+      const std::string prefix = cameraNamespace(camera_names_[camera_index]) + "/";
-            this->color_meta_Callback(msg, i);
+      auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-          });
+      callback_groups_.push_back(callback_group);
+      rclcpp::SubscriptionOptions options;
+      options.callback_group = callback_group;
 
-      ir_subscribers_.push_back(ir_sub);
+      for (const auto &stream_name : stream_names) {
-      ir_meta_subscribers_.push_back(ir_metadata_sub);
+        captures_[camera_index].emplace(stream_name, StreamCapture{});
-      color_subscribers_.push_back(color_sub);
+        const std::string image_topic = prefix + stream_name + "/image_raw";
-      color_meta_subscribers_.push_back(color_metadata_sub);
+        const std::string metadata_topic = prefix + stream_name + "/metadata";
+        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));
+        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));
+      }
     }
   }
-  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();
+  std::string currentDateTime() const {
-    return date_str;
+    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) {
-    std::string path = std::string("multicamera_sync/output/") + currenttimes_ + "/" +
-                       "TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" +
-                       std::to_string(serial_index);
 
+  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();
+  std::string receiveTimestamp() const {
-    int64_t seconds = now.seconds();
+    const auto now = this->get_clock()->now();
-    int64_t nanoseconds = now.nanoseconds() % 1000000000;
+    const int64_t seconds = now.seconds();
-    int64_t milliseconds = nanoseconds / 1000000;
+    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) {
+  bool captureReady(size_t camera_index) const {
-    auto &ir_images = ir_image_buffers_[index];
+    if (camera_index >= captures_.size() || captures_[camera_index].empty()) {
-    auto &ir_current_timestamps = ir_current_timestamp_buffers_[index];
+      return false;
-    auto &ir_timestamps = ir_timestamp_buffers_[index];
+    }
-    auto &color_images = color_image_buffers_[index];
+    return std::all_of(
-    auto &color_current_timestamps = color_current_timestamp_buffers_[index];
+        captures_[camera_index].begin(), captures_[camera_index].end(), [this](const auto &entry) {
-    auto &color_timestamps = color_timestamp_buffers_[index];
+          return entry.second.images.size() >= static_cast<size_t>(saving_images_number_);
-    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];
+  std::string metadataSuffix(const StreamCapture &capture, size_t frame_index) const {
-    callback_called_[index] = true;
+    std::string suffix;
-    if (ir_images.size() < static_cast<size_t>(saving_images_number_) ||
+    if (frame_index < capture.exposures.size()) {
-        color_images.size() < static_cast<size_t>(saving_images_number_)) {
+      suffix += "_e" + capture.exposures[frame_index];
+    }
+    if (frame_index < capture.gains.size()) {
+      suffix += "_d" + capture.gains[frame_index];
+    }
+    return suffix;
+  }
+
+  void saveImages(size_t camera_index) {
+    if (!captureReady(camera_index)) {
       return;
     }
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "index:" << index);
+    if (camera_index >= usb_ports_.size()) {
-    auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
+      RCLCPP_ERROR(get_logger(), "Missing USB port configuration for camera index %zu",
-    auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
+                   camera_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");
       return;
     }
-    std::string serial_index = serial_iter->second;
+    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++) {
+    for (const auto &entry : captures_[camera_index]) {
-      std::string folder = generateFolderName(serial_index, usb_index);
+      const std::string &stream_name = entry.first;
-      std::string ir_filename =
+      const auto &capture = entry.second;
-          folder + "/ir#left_SN" + serial_index + "_Index" + std::to_string(usb_index) +
+      for (size_t i = 0; i < static_cast<size_t>(saving_images_number_); ++i) {
-          time_domain_ + ir_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
+        if (capture.images[i].empty()) {
-          ir_timestamps[i] +
+          continue;
-          (is_gemini330_ ? ("_e" + left_ir_meta_exposure[i] + "_d" + left_ir_meta_gain[i]) : "") +
+        }
-          "_.jpg";
+        const std::string filename = folder + "/" + stream_name + "_SN" + serial_number + "_Index" +
-      if (ir_images[i].empty()) {
+                                     std::to_string(usb_index) + time_domain_suffix_ +
-        RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
+                                     capture.current_timestamps[i] + "_f" + std::to_string(i) +
-        continue;
+                                     "_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();
+    for (auto &entry : captures_[camera_index]) {
-    ir_current_timestamp_buffers_[index].clear();
+      entry.second.clear();
-    ir_timestamp_buffers_[index].clear();
+    }
-    color_image_buffers_[index].clear();
+    const bool all_cameras_complete = std::all_of(callback_called_.begin(), callback_called_.end(),
-    color_current_timestamp_buffers_[index].clear();
+                                                  [](bool value) { return value; });
-    color_timestamp_buffers_[index].clear();
+    if (all_cameras_complete) {
-    left_ir_metadata_.exposure_buffs[index].clear();
+      RCLCPP_INFO(get_logger(), "Capture completed for all cameras");
-    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 ");
       saving_images_number_ = 0;
-      callback_called_.clear();
+      callback_called_.assign(camera_names_.size(), false);
-      callback_called_ = std::vector<bool>(left_ir_topics_.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;
+    std::lock_guard<std::mutex> lock(capture_mutex_);
-    currenttimes_ = getCurrentTimes();
+    if (request->data <= 0) {
+      response->success = false;
+      response->message = "capture image count must be greater than zero";
+      return;
+    }
+    if (!topics_initialized_) {
+      initializeTopics();
+      topics_initialized_ = true;
+    }
+    if (captures_.empty() || std::any_of(captures_.begin(), captures_.end(),
+                                         [](const auto &streams) { return streams.empty(); })) {
+      response->success = false;
+      response->message = "no supported image streams are configured";
+      return;
+    }
+
+    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_) {
+    response->success = true;
-      topic_init();
+    response->message = "capture started";
-      topic_init_ = true;
+    RCLCPP_INFO(get_logger(), "Capturing %d image(s) from each configured stream",
-    }
+                saving_images_number_);
-    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_);
+  void imageCallback(const std::shared_ptr<const sensor_msgs::msg::Image> image,
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
+                     size_t camera_index, const std::string &stream_name) {
-      cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
+    std::lock_guard<std::mutex> lock(capture_mutex_);
-      std::string current_timestamp_ir = getCurrentTimestamp(image);
+    if (saving_images_number_ <= 0 || callback_called_[camera_index]) {
-      std::string timestamp_ir = getTimestamp();
+      return;
-      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);
-      }
     }
+    auto &capture = captures_[camera_index].at(stream_name);
+    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;
+    }
+    capture.images.push_back(output);
+    capture.current_timestamps.push_back(imageTimestamp(image));
+    capture.receive_timestamps.push_back(receiveTimestamp());
+    RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index,
+                capture.images.size(), saving_images_number_);
+    saveImages(camera_index);
   }
-  void colorCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
+
-    std::lock_guard<std::mutex> lock(image_mutex_);
+  void metadataCallback(const std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> metadata,
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
+                        size_t camera_index, const std::string &stream_name) {
-      cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
+    std::lock_guard<std::mutex> lock(capture_mutex_);
-      cv::Mat corrected_image;
+    if (saving_images_number_ <= 0 || callback_called_[camera_index]) {
-      cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR);
+      return;
-      std::string current_timestamp_color = getCurrentTimestamp(image);
+    }
-      std::string timestamp_color = getTimestamp();
+    try {
-      color_image_buffers_[index].push_back(corrected_image);
+      const auto json_data = nlohmann::json::parse(metadata->json_data);
-      color_current_timestamp_buffers_[index].push_back(current_timestamp_color);
+      auto &capture = captures_[camera_index].at(stream_name);
-      color_timestamp_buffers_[index].push_back(timestamp_color);
+      if (json_data.contains("exposure")) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
+        capture.exposures.push_back(json_data["exposure"].dump());
-                         ":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);
       }
+      if (json_data.contains("gain")) {
+        capture.gains.push_back(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());
     }
   }
 
-  void ir_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
+  std::mutex capture_mutex_;
-                        size_t index) {
+  std::vector<rclcpp::CallbackGroup::SharedPtr> callback_groups_;
-    std::lock_guard<std::mutex> lock(meta_mutex_);
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> image_subscribers_;
-    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 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());
-    }
-  }
-
   std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
-      ir_meta_subscribers_;
+      metadata_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_;
   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_ports_;
-
+  std::vector<std::string> camera_names_;
-  std::vector<std::string> usb_params_;
+  std::vector<std::string> configured_stream_names_;
-  std::vector<std::string> camera_name_;
+  std::vector<std::map<std::string, StreamCapture>> captures_;
-  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<bool> callback_called_;
-
+  std::string time_domain_suffix_;
-  std::string currenttimes_;
+  std::string current_date_time_;
-
+  int saving_images_number_ = 0;
-  int saving_images_number_ = 100;
+  bool topics_initialized_ = false;
-
+  bool has_gemini330_device_ = false;
-  bool topic_init_ = false;
-  bool is_gemini330_ = true;
-
-  ImageMetadata left_ir_metadata_ = ImageMetadata();
-  ImageMetadata color_metadata_ = ImageMetadata();
 };
+
 }  // 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"),