Delete orb_device_lock when the program exits、list_devices_node add USB port type and update multi_save

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -105,6 +105,11 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
     reset_device_thread_->join();
   }
   ob_camera_node_->stopGmslTrigger();
+  if (orb_device_lock_shm_fd_ != -1) {
+    close(orb_device_lock_shm_fd_);
+    orb_device_lock_shm_fd_ = -1;
+  }
+  shm_unlink(ORB_DEFAULT_LOCK_NAME.c_str());
 }
 
 void OBCameraNodeDriver::init() {
@@ -292,8 +297,7 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
   } else if (!usb_port_.empty()) {
     RCLCPP_INFO_STREAM(logger_, "Connecting to device with usb port: " << usb_port_);
     device = selectDeviceByUSBPort(list, usb_port_);
-  }
+  } else if (device_num_ == 1) {
-    else if (device_num_ == 1) {
     RCLCPP_INFO_STREAM(logger_, "Connecting to the default device");
     return list->getDevice(0);
   }

orbbec_camera/tools/list_devices_node.cpp

@@ -31,6 +31,7 @@ int main() {
       auto usb_port = orbbec_camera::parseUsbPort(uid);
       RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "serial: " << serial);
       RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "usb port: " << usb_port);
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "usb connect type: " << device_info->getConnectionType());
     }
   }  catch (ob::Error &e) {
     RCLCPP_ERROR_STREAM(rclcpp::get_logger("list_device_node"), e.getMessage());

orbbec_camera/tools/multi_save_rgbir_node.cpp

@@ -18,7 +18,8 @@
 int main(int argc, char **argv) {
   rclcpp::init(argc, argv);
   auto node = std::make_shared<MultiCameraSubscriber>();
-  rclcpp::spin(node);
+  rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20);
-  rclcpp::shutdown();
+  executor.add_node(node);
+  executor.spin();
   return 0;
 }

orbbec_camera/tools/multi_save_rgbir_node.hpp

@@ -6,11 +6,17 @@
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/synchronizer.h>
+#include <std_msgs/msg/bool.hpp>
 #include <filesystem>
 
 class MultiCameraSubscriber : public rclcpp::Node {
  public:
   MultiCameraSubscriber() : Node("multi_camera_subscriber") {
+    device_init();
+    currenttimes_=getCurrentTimes();
+
+  }
+  void device_init() {
     try {
       auto context = std::make_unique<ob::Context>();
       context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
@@ -21,13 +27,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
         std::string serial = device_info->serialNumber();
         std::string uid = device_info->uid();
         auto usb_port = orbbec_camera::parseUsbPort(uid);
-        int vid = device_info->vid();
-        int pid = device_info->pid();
         serial_numbers_[usb_port] = serial;
-        RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_save_rgbir_node"), ":vid: " << std::hex << vid);
+        // RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":list->deviceCount(): " << list->deviceCount());
-        RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_save_rgbir_node"), ":pid: " << std::hex << pid);
-        RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_save_rgbir_node"), ":serial: " << serial);
-        RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_save_rgbir_node"), ":usb_port: " << usb_port);
         color_frame_counters_[count] = 0;
         ir_frame_counters_[count] = 0;
         count++;
@@ -43,17 +44,23 @@ class MultiCameraSubscriber : public rclcpp::Node {
     this->declare_parameter<std::vector<std::string>>("ir_topics", std::vector<std::string>());
     this->declare_parameter<std::vector<std::string>>("color_topics", std::vector<std::string>());
     this->declare_parameter<std::vector<std::string>>("usb_ports", std::vector<std::string>());
+    this->declare_parameter<std::string>("image_number", "100");
 
-    std::vector<std::string> ir_topics_ = this->get_parameter("ir_topics").as_string_array();
+    ir_topics_ = this->get_parameter("ir_topics").as_string_array();
-    std::vector<std::string> color_topics_ = this->get_parameter("color_topics").as_string_array();
+    color_topics_ = this->get_parameter("color_topics").as_string_array();
-    std::vector<std::string> usb_params_ = this->get_parameter("usb_ports").as_string_array();
+    usb_params_ = this->get_parameter("usb_ports").as_string_array();
+    image_number_ = this->get_parameter("image_number").as_string();
 
     auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
+    capture_control_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "start_capture", custom_qos,
+        std::bind(&MultiCameraSubscriber::controlCaptureCallback, this, std::placeholders::_1));
     for (size_t i = 0; i < usb_params_.size(); i++) {
       usb_numbers_[i] = usb_params_[i];
       usb_index_map_[usb_params_[i]] = i;
     }
-
+    reentrant_callback_group_ =
+        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
     for (const auto &pair : serial_numbers_) {
       RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
                          "usb_port: " << pair.first << ", serial: " << pair.second);
@@ -62,44 +69,68 @@ class MultiCameraSubscriber : public rclcpp::Node {
       RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
                          "usb_port: " << pair.first << ", index: " << pair.second);
     }
+  }
 
+ private:
+  std::mutex buffer_mutex_;
+  void topic_init() {
+    auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
+                       "color_topic: " << ir_topics_.size());
     for (size_t i = 0; i < ir_topics_.size(); ++i) {
       RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
                          "ir_topic: " << ir_topics_[i]);
       RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
                          "color_topic: " << color_topics_[i]);
 
-      auto ir_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
+      rclcpp::SubscriptionOptions ir_sub_options;
-          this, ir_topics_[i], custom_qos.get_rmw_qos_profile());
+      ir_sub_options.callback_group = reentrant_callback_group_;
-      auto color_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
-          this, color_topics_[i], custom_qos.get_rmw_qos_profile());
 
-      ir_sub->registerCallback([this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
+      rclcpp::SubscriptionOptions color_sub_options;
-        this->irCallback(msg, i);
+      color_sub_options.callback_group = reentrant_callback_group_;
-      });
 
-      color_sub->registerCallback([this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
+      auto ir_sub = this->create_subscription<sensor_msgs::msg::Image>(
-        this->colorCallback(msg, i);
+          ir_topics_[i], custom_qos,
-      });
+          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+            this->irCallback(msg, i);
+          },
+          ir_sub_options);
+
+      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);
 
       ir_subscribers_.push_back(ir_sub);
       color_subscribers_.push_back(color_sub);
+
+      ir_image_buffers_.resize(ir_topics_.size());
+      color_image_buffers_.resize(ir_topics_.size());
+      ir_current_timestamp_buffers_.resize(ir_topics_.size());
+      color_current_timestamp_buffers_.resize(ir_topics_.size());
+      ir_timestamp_buffers_.resize(ir_topics_.size());
+      color_timestamp_buffers_.resize(ir_topics_.size());
+      callback_called_ = std::vector<bool>(ir_topics_.size(), false);
     }
   }
+  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");
 
- private:
+    std::string date_str = date_stream.str();
-  std::string generateFolderName(const sensor_msgs::msg::Image::ConstSharedPtr &color_msg,
+    return date_str;
-                                 const std::string &serial_number, size_t serial_index) {
+  }
-    std::string color_resolution =
+  std::string generateFolderName(const std::string &serial_number, size_t serial_index) {
-        std::to_string(color_msg->width) + "x" + std::to_string(color_msg->height);
-    std::string color_encoding = color_msg->encoding;
-    std::string frame_rate = "30fps";
-    std::string folder_name = "Star-AE-OFF-ir-" + color_resolution + "-" + "y8" + "-rgb-" +
-                              color_resolution + "-" + "mjpg" + "-" + frame_rate;
 
-    std::string path = std::string("multicamera_sync/output/") + folder_name + "/" +
+    std::string path = std::string("multicamera_sync/output/") + currenttimes_ + "/" +
                        "TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" +
                        std::to_string(serial_index);
+
     std::filesystem::create_directories(path);
     return path;
   }
@@ -121,44 +152,130 @@ class MultiCameraSubscriber : public rclcpp::Node {
 
     return timestamp.str();
   }
-  void irCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) {
+
+  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];
+    callback_called_[index] = true;
+    if (ir_images.size() < static_cast<size_t>(std::stoi(image_number_)) ||
+        color_images.size() < static_cast<size_t>(std::stoi(image_number_))) {
+      return;
+    }
+
     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()) {
+      return;
+    }
     std::string serial_index = serial_iter->second;
-    cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
+
-    std::string timestamp_ir = getCurrentTimestamp(image);
+    for (size_t i = 0; i < static_cast<size_t>(std::stoi(image_number_)); i++) {
-    size_t frame_index = ir_frame_counters_[index]++;
+      std::string folder = generateFolderName(serial_index, usb_index);
-    std::string folder = generateFolderName(image, serial_index, usb_index);
+      std::string ir_filename = folder + "/ir#left_SN" + serial_index + "_Index" +
-    std::string filename = folder + "/ir#left_SN" + serial_index + "_Index" +
+                                std::to_string(usb_index) + "_d" + ir_current_timestamps[i] + "_f" +
-                           std::to_string(usb_index) + "_d" + timestamp_ir + "_f" +
+                                std::to_string(i) + "_s" + ir_timestamps[i] + "_.jpg";
-                           std::to_string(frame_index) + "_s" + getTimestamp() + "_.jpg";
+      if (ir_images[i].empty()) {
-    cv::imwrite(filename, ir_mat);
+        RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "over ");
-    RCLCPP_INFO(this->get_logger(), "Saved IR image for camera to: %s", filename.c_str());
+        // rclcpp::shutdown();
+        continue;
+      }
+
+      cv::imwrite(ir_filename, ir_images[i]);
+      // RCLCPP_INFO(this->get_logger(), "Saved IR image to: %s", ir_filename.c_str());
+
+      std::string color_filename = folder + "/color_SN" + serial_index + "_Index" +
+                                   std::to_string(usb_index) + "_d" + color_current_timestamps[i] +
+                                   "_f" + std::to_string(i) + "_s" + color_timestamps[i] + "_.jpg";
+      if (ir_images[i].empty()) {
+        // rclcpp::shutdown();
+        continue;
+      }
+      cv::imwrite(color_filename, color_images[i]);
+      // RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str());
+    }
+
+    ir_images.clear();
+    color_images.clear();
+    ir_current_timestamps.clear();
+    color_current_timestamps.clear();
+    ir_timestamps.clear();
+    color_timestamps.clear();
+    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();
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
+                       "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("list_device_node"), "over ");
+      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();
+      rclcpp::shutdown();
+    }
   }
 
-  void colorCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) {
+  void controlCaptureCallback(const std_msgs::msg::Bool::SharedPtr msg) {
-    auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
+    is_saving_images_ = msg->data;
-    auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
+    topic_init();
-    int usb_index = usb_iter->second;
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjj " << is_saving_images_);
-    std::string serial_index = serial_iter->second;
+  }
-    cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
+  void irCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
-    cv::Mat corrected_image;
+    std::lock_guard<std::mutex> lock(buffer_mutex_);
-    cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR);
+    if (!callback_called_[index] && is_saving_images_) {
-    std::string timestamp_color = getCurrentTimestamp(image);
+      cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
-    size_t frame_index = color_frame_counters_[index]++;
+      std::string current_timestamp_ir = getCurrentTimestamp(image);
-    std::string folder = generateFolderName(image, serial_index, usb_index);
+      std::string timestamp_ir = getTimestamp();
-    std::string filename = folder + "/color_SN" + serial_index + "_Index" +
+      ir_image_buffers_[index].push_back(ir_mat);
-                           std::to_string(usb_index) + "_d" + timestamp_color + "_f" +
+      ir_current_timestamp_buffers_[index].push_back(current_timestamp_ir);
-                           std::to_string(frame_index) + "_s" + getTimestamp() + "_.jpg";
+      ir_timestamp_buffers_[index].push_back(timestamp_ir);
-    cv::imwrite(filename, corrected_image);
+      ir_resolution_ = std::to_string(image->width) + "x" + std::to_string(image->height);
-    RCLCPP_INFO(this->get_logger(), "Saved Color image for camera to: %s", filename.c_str());
+    //   RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":ir: " << index <<":"<<
+    //   ir_image_buffers_[index].size());
+      if (ir_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_)) &&
+          color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
+        saveAlignedImages(index);
+      }
+    }
   }
 
-  std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>>>
+  void colorCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
-      ir_subscribers_;
+    std::lock_guard<std::mutex> lock(buffer_mutex_);
-  std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>>>
+    if (!callback_called_[index] && is_saving_images_) {
-      color_subscribers_;
+      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);
+      color_resolution_ = std::to_string(image->width) + "x" + std::to_string(image->height);
+    //   RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":color: " << index <<":"<<
+    //   color_image_buffers_[index].size());
+      if (ir_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_)) &&
+          color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
+        saveAlignedImages(index);
+      }
+    }
+  }
+
+  rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> ir_subscribers_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subscribers_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr capture_control_sub_;
 
   std::map<std::string, int> usb_index_map_;
   std::map<std::string, std::string> serial_numbers_;
@@ -170,4 +287,20 @@ class MultiCameraSubscriber : public rclcpp::Node {
   std::vector<std::string> usb_params_;
   std::vector<std::string> ir_topics_;
   std::vector<std::string> color_topics_;
-};
+  std::string image_number_;
+
+  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 color_resolution_;
+  std::string ir_resolution_;
+ std::string currenttimes_;
+
+  bool is_saving_images_ = false;
+};