fix: support side streams in camera tools

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,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 "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 <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 {
-  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",
+};
+
+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();
-  }
-  ~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_ = 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");
+    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,9 +86,27 @@ class MultiCameraSubscriber : public rclcpp::Node {
   }
 
  private:
-  std::mutex image_mutex_;
-  std::mutex meta_mutex_;
-  bool isGemini335PID(uint32_t pid) {
+  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();
+        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>();
-    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<std::string> discoverStreamNames(const std::string &camera_name) const {
+    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>(
-          left_ir_topics_[i], custom_qos,
-          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
-            this->irCallback(msg, i);
-          },
-          ir_sub_options);
+  std::vector<std::string> selectStreamNames(const std::string &camera_name) const {
+    if (!configured_stream_names_.empty()) {
+      return configured_stream_names_;
+    }
+    auto stream_names = discoverStreamNames(camera_name);
+    if (!stream_names.empty()) {
+      return stream_names;
+    }
 
-      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);
-          });
+    RCLCPP_WARN(get_logger(),
+                "No supported image topics discovered for %s; using legacy RGB/IR topics",
+                camera_name.c_str());
+    return {has_gemini330_device_ ? "left_ir" : "ir", "color"};
+  }
 
-      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);
+  void initializeTopics() {
+    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));
 
-      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);
-          });
+    for (size_t camera_index = 0; camera_index < camera_names_.size(); ++camera_index) {
+      const auto stream_names = selectStreamNames(camera_names_[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;
 
-      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);
+      for (const auto &stream_name : stream_names) {
+        captures_[camera_index].emplace(stream_name, StreamCapture{});
+        const std::string image_topic = prefix + stream_name + "/image_raw";
+        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();
-    return date_str;
+  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) {
-    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();
-    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_);
+        });
+  }
+
+  std::string metadataSuffix(const StreamCapture &capture, size_t frame_index) const {
+    std::string suffix;
+    if (frame_index < capture.exposures.size()) {
+      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);
-    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");
+    if (camera_index >= usb_ports_.size()) {
+      RCLCPP_ERROR(get_logger(), "Missing USB port configuration for camera index %zu",
+                   camera_index);
       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++) {
-      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_) {
+      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_) {
-      topic_init();
-      topic_init_ = true;
-    }
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                       "saving_images_number_: " << saving_images_number_);
+    response->success = true;
+    response->message = "capture started";
+    RCLCPP_INFO(get_logger(), "Capturing %d image(s) from each configured stream",
+                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 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;
     }
+    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_);
-    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);
+
+  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;
+    }
+    try {
+      const auto json_data = nlohmann::json::parse(metadata->json_data);
+      auto &capture = captures_[camera_index].at(stream_name);
+      if (json_data.contains("exposure")) {
+        capture.exposures.push_back(json_data["exposure"].dump());
       }
+      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,
-                        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 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::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;
+  bool has_gemini330_device_ = 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"),