[test]Add multi_save_cloud_node

orbbec_camera/tools/multi_save_cloud_node.cpp

@@ -0,0 +1,25 @@
+/*******************************************************************************
+ * 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_cloud_node.hpp"
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<orbbec_camera::tools::MultiCameraCloudSubscriber>();
+  rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 8);
+  executor.add_node(node);
+  executor.spin();
+  return 0;
+}

orbbec_camera/tools/multi_save_cloud_node.hpp

@@ -0,0 +1,109 @@
+#pragma once
+#include <rclcpp/rclcpp.hpp>
+
+#include <orbbec_camera/ob_camera_node_driver.h>
+#include <orbbec_camera/ob_camera_node.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 <std_msgs/msg/int32.hpp>
+#include <filesystem>
+
+namespace orbbec_camera {
+namespace tools {
+
+class MultiCameraCloudSubscriber : public rclcpp::Node {
+ public:
+  MultiCameraCloudSubscriber() : Node("multi_camera_cloud_subscriber") { topic_init(); }
+  //   ~MultiCameraCloudSubscriber() {
+  //   }
+
+ private:
+  std::mutex image_mutex_;
+  void topic_init() {
+    auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
+    current_path = std::filesystem::current_path().string();
+    stand_cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+        "/camera/depth/points", custom_qos,
+        [this](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
+          this->stand_cloud_Callback(msg);
+        });
+    transform_cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+        "/rear_camera/depth/points", custom_qos,
+        [this](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
+          this->transform_cloud_Callback(msg);
+        });
+  }
+
+  void stand_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    std::stringstream ss;
+    auto now = std::time(nullptr);
+    ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
+    std::string filename = current_path + "/point_cloud/points_" + ss.str() + ".ply";
+    if (!std::filesystem::exists(current_path + "/point_cloud")) {
+      std::filesystem::create_directory(current_path + "/point_cloud");
+    }
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_cloud_subscriber"), "Saving point cloud to " << filename);
+    saveDepthPointsToPly(msg, filename);
+
+  }
+void transform_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    std::stringstream ss;
+    auto now = std::time(nullptr);
+    ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
+    std::string filename = current_path + "/rear_camera/point_cloud/points_" + ss.str() + ".ply";
+    if (!std::filesystem::exists(current_path + "/point_cloud")) {
+      std::filesystem::create_directory(current_path + "/point_cloud");
+    }
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_cloud_subscriber"),
+                       "Saving transform_cloud to " << filename);
+    saveDepthPointsToPly(msg, filename);
+  }
+  void saveDepthPointsToPly(const std::shared_ptr<const sensor_msgs::msg::PointCloud2> &msg,
+                          const std::string &fileName) {
+  FILE *fp = fopen(fileName.c_str(), "wb+");
+  CHECK_NOTNULL(fp);
+  CHECK_NOTNULL(msg);
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
+
+  // First, count the actual number of valid points
+  size_t valid_points = 0;
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
+      ++valid_points;
+    }
+  }
+
+  // Reset the iterators
+  iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
+  iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
+  iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");
+
+  fprintf(fp, "ply\n");
+  fprintf(fp, "format ascii 1.0\n");
+  fprintf(fp, "element vertex %zu\n", valid_points);
+  fprintf(fp, "property float x\n");
+  fprintf(fp, "property float y\n");
+  fprintf(fp, "property float z\n");
+  fprintf(fp, "end_header\n");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
+      fprintf(fp, "%.3f %.3f %.3f\n", *iter_x, *iter_y, *iter_z);
+    }
+  }
+
+  fflush(fp);
+  fclose(fp);
+}
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr stand_cloud_sub_;
+  std::string current_path;
+};
+}  // namespace tools
+}  // namespace orbbec_camera