Update OrbbecSDK_ROS2 to v2.3.0

orbbec_camera/tools/start_benchmark.cpp

@@ -0,0 +1,148 @@
+#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_msgs/msg/metadata.hpp"
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <filesystem>
+
+namespace orbbec_camera {
+namespace tools {
+
+class StartBenchmark : public rclcpp::Node {
+ public:
+  explicit StartBenchmark(const rclcpp::NodeOptions& options) : Node("StartBenchmark", options) {
+    params_init();
+    auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
+    for (size_t i = 0; i < camera_name_.size(); ++i) {
+      color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
+          color_topics_[i], custom_qos,
+          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+            this->color_Callback(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) {
+            this->depth_Callback(msg, i);
+          }));
+      left_ir_subs_.push_back(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->left_ir_Callback(msg, i);
+          }));
+      right_ir_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
+          right_ir_topics_[i], custom_qos,
+          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+            this->right_ir_Callback(msg, i);
+          }));
+      depth_point_cloud_subs_.push_back(this->create_subscription<sensor_msgs::msg::PointCloud2>(
+          depth_point_cloud_topics_[i], custom_qos,
+          [this, i](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
+            this->depth_point_cloud_Callback(msg, i);
+          }));
+      color_point_cloud_subs_.push_back(this->create_subscription<sensor_msgs::msg::PointCloud2>(
+          color_point_cloud_topics_[i], custom_qos,
+          [this, i](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
+            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"), 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 ");
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
+                         depth_point_cloud_topics_[i] << " is subed ");
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
+                         color_point_cloud_topics_[i] << " is subed ");
+    }
+  }
+
+ private:
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> 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_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
+      depth_point_cloud_subs_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
+      color_point_cloud_subs_;
+
+
+  std::vector<std::string> camera_name_;
+  std::vector<std::string> color_topics_;
+  std::vector<std::string> depth_topics_;
+  std::vector<std::string> left_ir_topics_;
+  std::vector<std::string> right_ir_topics_;
+  std::vector<std::string> depth_point_cloud_topics_;
+  std::vector<std::string> color_point_cloud_topics_;
+
+  std::mutex image_mutex_;
+
+  void params_init() {
+    std::ifstream file(
+        "install/orbbec_camera/share/orbbec_camera/config/tools/startbenchmark/"
+        "start_benchmark_params.json");
+    if (!file.is_open()) {
+      RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open JSON file.");
+      return;
+    }
+    nlohmann::json json_data;
+    file >> json_data;
+    camera_name_ =
+        json_data["start_benchmark_params"]["camera_name"].get<std::vector<std::string>>();
+    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());
+    depth_point_cloud_topics_.resize(camera_name_.size());
+    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";
+      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";
+      depth_point_cloud_topics_[i] = "/" + camera_name_[i] + "/depth/points";
+      color_point_cloud_topics_[i] = "/" + camera_name_[i] + "/depth_registered/points";
+    }
+  }
+
+  void color_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"),
+                        "time is : " << msg->step << "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"),
+                        "time is : " << msg->step << "depth is subed " << index << "is subed");
+  }
+  void left_ir_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"),
+                        "time is : " << msg->step << "left_ir is subed " << index << "is subed");
+  }
+  void right_ir_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"),
+                        "time is : " << msg->step << "right_ir is subed " << index << "is subed");
+  }
+  void depth_point_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg,
+                                  size_t index) {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    RCLCPP_DEBUG_STREAM(
+        rclcpp::get_logger("StartBenchmark"),
+        "time is : " << msg->point_step << "depth_point_cloud is subed " << index << "is subed");
+  }
+  void color_point_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg,
+                                  size_t index) {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    RCLCPP_DEBUG_STREAM(
+        rclcpp::get_logger("StartBenchmark"),
+        "time is : " << msg->point_step << "color_point_cloud is subed " << index << "is subed");
+  }
+};
+
+}  // namespace tools
+}  // namespace orbbec_camera
+RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::tools::StartBenchmark)