Update save_rgbir tool

orbbec_camera/CMakeLists.txt

@@ -215,7 +215,6 @@ add_orbbec_executable(list_devices_node tools/list_devices_node.cpp)
 add_orbbec_executable(list_depth_work_mode_node tools/list_depth_work_mode.cpp)
 add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cpp)
 add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp)
-add_orbbec_executable(multi_save_rgbir_node tools/multi_save_rgbir_node.cpp)
 add_orbbec_executable(metadata_save_files_node tools/metadata_save_files.cpp)
 add_orbbec_executable(metadata_export_files_node tools/metadata_export_files.cpp)
 add_orbbec_executable(multi_save_cloud_node tools/multi_save_cloud_node.cpp)
@@ -230,6 +229,15 @@ rclcpp_components_register_node(frame_latency
         EXECUTABLE frame_latency_node
 )
 
+add_library(multi_save_rgbir SHARED tools/multi_save_rgbir.cpp)
+target_include_directories(multi_save_rgbir PUBLIC ${COMMON_INCLUDE_DIRS} )
+target_link_libraries(multi_save_rgbir ${COMMON_LIBRARIES})
+ament_target_dependencies(multi_save_rgbir ${dependencies})
+
+rclcpp_components_register_node(multi_save_rgbir
+        PLUGIN "orbbec_camera::tools::MultiCameraSubscriber"
+        EXECUTABLE multi_save_rgbir_node
+)
 # Install rules
 install(TARGETS ${PROJECT_NAME} frame_latency
   ARCHIVE DESTINATION lib
@@ -254,7 +262,6 @@ install(TARGETS list_devices_node
   list_depth_work_mode_node
   list_camera_profile_mode_node
   topic_statistics_node
-  multi_save_rgbir_node
   metadata_save_files_node
   metadata_export_files_node
   multi_save_cloud_node

orbbec_camera/launch/multi_camera_synced.launch.py

@@ -8,6 +8,8 @@ from launch.actions import DeclareLaunchArgument
 from launch.conditions import UnlessCondition, IfCondition
 from launch.substitutions import LaunchConfiguration
 from launch.substitutions import TextSubstitution
+from launch_ros.actions import Node, LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
 
 def generate_launch_description():
     # Include launch files
@@ -38,6 +40,18 @@ def generate_launch_description():
         condition=UnlessCondition(attach_to_shared_component_container_arg)
     )
 
+    save_rgbir = LoadComposableNodes(
+        target_container=component_container_name_arg,
+        composable_node_descriptions=[
+            ComposableNode(
+                namespace="save_rgbir",
+                name="save_rgbir",
+                package="orbbec_camera",
+                plugin="orbbec_camera::tools::MultiCameraSubscriber",
+            )
+        ],
+    )
+
     attach_to_shared_component_container_arg = TextSubstitution(text='true')
 
     front_camera = IncludeLaunchDescription(
@@ -124,6 +138,10 @@ def generate_launch_description():
         period=8.0,
         actions=[front_camera],
     )
+    delayed_save_rgbir = TimerAction(
+        period=16.0,
+        actions=[save_rgbir],
+    )
     ld = LaunchDescription(
       [
         use_intra_process_comms_declare,
@@ -134,6 +152,7 @@ def generate_launch_description():
         delayed_right_camera,
         delayed_rear_camera,
         delayed_front_camera,
+        delayed_save_rgbir,
       ]
     )
 

orbbec_camera/tools/multi_save_rgbir.cpp

@@ -1,14 +1,16 @@
-#pragma once
-#include <rclcpp/rclcpp.hpp>
 
+#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 <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/synchronizer.h>
 #include <std_msgs/msg/int32.hpp>
 #include <filesystem>
+#include <regex>
 namespace orbbec_camera {
 namespace tools {
 struct ImageMetadata {
@@ -18,7 +20,10 @@ struct ImageMetadata {
 
 class MultiCameraSubscriber : public rclcpp::Node {
  public:
-  MultiCameraSubscriber() : Node("multi_camera_subscriber") { device_init(); }
+  explicit MultiCameraSubscriber(const rclcpp::NodeOptions &options)
+      : Node("MultiCameraSubscriber", options) {
+    device_init();
+  }
   ~MultiCameraSubscriber() {
     ir_image_buffers_.clear();
     ir_current_timestamp_buffers_.clear();
@@ -41,10 +46,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
         auto device_info = device->getDeviceInfo();
         std::string serial = device_info->serialNumber();
         std::string uid = device_info->uid();
-        auto usb_port = orbbec_camera::parseUsbPort(uid);
+        auto usb_port = parseUsbPort(uid);
         serial_numbers_[usb_port] = serial;
-        // RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":list->deviceCount(): " <<
-        // list->deviceCount());
         color_frame_counters_[count_] = 0;
         ir_frame_counters_[count_] = 0;
         count_++;
@@ -70,15 +73,14 @@ class MultiCameraSubscriber : public rclcpp::Node {
       RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
                          "usb_port: " << pair.first << ", index: " << pair.second);
     }
-    auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
-    capture_control_sub_ = this->create_subscription<std_msgs::msg::Int32>(
-        "start_capture", custom_qos,
-        std::bind(&MultiCameraSubscriber::controlCaptureCallback, this, std::placeholders::_1));
+    capture_control_srv_ = this->create_service<orbbec_camera_msgs::srv::SetInt32>(
+        "start_capture", std::bind(&MultiCameraSubscriber::controlCaptureCallback, this,
+                                   std::placeholders::_1, std::placeholders::_2));
   }
 
  private:
   std::mutex image_mutex_;
-//   std::mutex meta_mutex_;
+  std::mutex meta_mutex_;
   void params_init() {
     std::ifstream file(
         "install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"
@@ -105,6 +107,41 @@ class MultiCameraSubscriber : public rclcpp::Node {
       color_metadata_topic_[i] = "/" + camera_name_[i] + "/color/metadata";
     }
   }
+  std::string parseUsbPort(const std::string &line) {
+    std::string port_id;
+    std::regex usb_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*",
+                         std::regex_constants::ECMAScript);
+    std::smatch base_match;
+    bool found_usb = std::regex_match(line, base_match, usb_regex);
+
+    if (found_usb) {
+      port_id = base_match[1].str();
+      std::cout << "USB port_id: " << port_id << std::endl;
+
+      if (base_match[2].str().empty()) {
+        std::regex end_regex(".+(-[0-9]+$)", std::regex_constants::ECMAScript);
+        bool found_end = std::regex_match(port_id, base_match, end_regex);
+
+        if (found_end) {
+          port_id = port_id.substr(0, port_id.size() - base_match[1].str().size());
+          std::cout << "Modified USB port_id: " << port_id << std::endl;
+        }
+      }
+
+      return port_id;
+    }
+
+    std::regex gmsl_regex("(gmsl[0-9]+)(?:-[0-9]+)*(-[0-9]+)$", std::regex_constants::ECMAScript);
+    bool found_gmsl = std::regex_match(line, base_match, gmsl_regex);
+
+    if (found_gmsl) {
+      port_id = base_match[1].str() + base_match[2].str();
+      std::cout << "Parsed GMSL Port ID: " << port_id << std::endl;
+      return port_id;
+    }
+
+    return "";
+  }
   void topic_init() {
     ir_image_buffers_.resize(left_ir_topics_.size());
     color_image_buffers_.resize(left_ir_topics_.size());
@@ -117,8 +154,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
     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_sensor_data));
-    custom_qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
+    auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
     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) {
@@ -278,9 +314,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
     }
   }
 
-  void controlCaptureCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+  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();
-    saving_images_number_ = msg->data;
+    saving_images_number_ = request->data;
     if (!topic_init_) {
       topic_init();
       topic_init_ = true;
@@ -337,7 +376,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
 
   void ir_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
                         size_t index) {
-    std::lock_guard<std::mutex> lock(image_mutex_);
+    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());
@@ -346,7 +385,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
   }
   void color_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
                            size_t index) {
-    std::lock_guard<std::mutex> lock(image_mutex_);
+    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());
@@ -361,7 +400,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
       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::Subscription<std_msgs::msg::Int32>::SharedPtr capture_control_sub_;
+  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_;
@@ -391,7 +430,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
   std::string currenttimes_;
 
   size_t count_ = 0;
-  int saving_images_number_ = 0;
+  int saving_images_number_ = 100;
 
   bool topic_init_ = false;
 
@@ -399,4 +438,5 @@ class MultiCameraSubscriber : public rclcpp::Node {
   ImageMetadata color_metadata_ = ImageMetadata();
 };
 }  // namespace tools
-}  // namespace orbbec_camera
+}  // namespace orbbec_camera
+RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::tools::MultiCameraSubscriber)

orbbec_camera/tools/multi_save_rgbir_node.cpp

@@ -1,25 +0,0 @@
-/*******************************************************************************
- * Copyright (c) 2023 Orbbec 3D Technology, Inc
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *******************************************************************************/
-#include "multi_save_rgbir_node.hpp"
-
-int main(int argc, char **argv) {
-  rclcpp::init(argc, argv);
-  auto node = std::make_shared<orbbec_camera::tools::MultiCameraSubscriber>();
-  rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20);
-  executor.add_node(node);
-  executor.spin();
-  return 0;
-}