use single thread publish point cloud

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -48,6 +48,8 @@
 #include "orbbec_camera/constants.h"
 #include "orbbec_camera/dynamic_params.h"
 
+#include "orbbec_camera/ob_point_cloud_publisher.h"
+
 #define STREAM_NAME(sip)                                                                       \
   (static_cast<std::ostringstream&&>(std::ostringstream()                                      \
                                      << _stream_name[sip.first]                                \
@@ -305,5 +307,6 @@ class OBCameraNode {
   std::shared_ptr<std::thread> tf_thread_;
   std::condition_variable tf_cv_;
   double tf_publish_rate_ = 10.0;
+  std::unique_ptr<OBPointCloudPublisher> ob_point_cloud_publisher_;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_point_cloud_publisher.h

@@ -0,0 +1,42 @@
+#pragma once
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <thread>
+#include <condition_variable>
+#include <queue>
+#include <thread>
+#include <atomic>
+
+namespace orbbec_camera {
+class OBPointCloudPublisher {
+ public:
+  explicit OBPointCloudPublisher(rclcpp::Node* node, size_t max_filter_size = 2);
+
+  ~OBPointCloudPublisher();
+
+  void pushPointCloud(sensor_msgs::msg::PointCloud2&& point_cloud2);
+
+  void pushColorPointCloud(sensor_msgs::msg::PointCloud2&& point_cloud2);
+
+  void publishPointCloud();
+
+  void publishColorPointCloud();
+
+ private:
+  rclcpp::Node* node_;
+  rclcpp::Logger logger_;
+  std::atomic_bool is_alive_{false};
+  size_t max_filter_size_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_publisher_;
+  std::queue<sensor_msgs::msg::PointCloud2> point_cloud_q_;
+  std::mutex point_cloud_q_lock_;
+  std::condition_variable point_cloud_cv_;
+  std::thread point_cloud_thread_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr color_point_cloud_publisher_;
+  std::queue<sensor_msgs::msg::PointCloud2> color_point_cloud_q_;
+  std::mutex color_point_cloud_q_lock_;
+  std::condition_variable color_point_cloud_cv_;
+  std::thread color_point_cloud_thread_;
+};
+}  // namespace orbbec_camera