chore: switch to ros pointcloud type from PCL, use point publisher class

Signed-off-by: Max SCHMELLER <[email protected]>

nebula_decoders/include/nebula_decoders/nebula_decoders_common/managed_point_cloud.hpp

@@ -18,8 +18,6 @@
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
-#include <pcl/PCLPointCloud2.h>
-#include <pcl/conversions.h>
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <algorithm>
@@ -141,18 +139,10 @@ class ManagedPointCloud
    *
    * @return pcl::PCLPointCloud2 The popped pointcloud
    */
-  pcl::PCLPointCloud2 pop_cloud()
+  sensor_msgs::msg::PointCloud2 pop_cloud()
   {
-    pcl::PCLPointCloud2 result;
-    result.header = cloud_.header;
-    result.point_step = cloud_.point_step;
-    result.row_step = cloud_.row_step;
-    result.width = cloud_.width;
-    result.height = cloud_.height;
-    result.is_bigendian = cloud_.is_bigendian;
-    result.is_dense = cloud_.is_dense;
-    result.fields = cloud_.fields;
-    std::swap(result.data, cloud_.data);
+    sensor_msgs::msg::PointCloud2 result;
+    std::swap(result, cloud_);
     cloud_.data.clear();
     reset();
     return result;
@@ -163,23 +153,23 @@ class ManagedPointCloud
    *
    * @return const pcl::PCLPointCloud2& The current pointcloud
    */
-  const pcl::PCLPointCloud2 & peek_cloud() { return cloud_; }
+  const sensor_msgs::msg::PointCloud2 & peek_cloud() { return cloud_; }
 
 private:
   NebulaPoint & operator[](size_t index)
   {
     return *reinterpret_cast<NebulaPoint *>(&cloud_.data[index * sizeof(NebulaPoint)]);
   }
 
-  static pcl::PCLPointCloud2 get_empty_cloud()
+  static sensor_msgs::msg::PointCloud2 get_empty_cloud()
   {
     NebulaPointCloud cloud_template;
-    pcl::PCLPointCloud2 cloud;
-    pcl::toPCLPointCloud2(cloud_template, cloud);
+    sensor_msgs::msg::PointCloud2 cloud;
+    pcl::toROSMsg(cloud_template, cloud);
     return cloud;
   }
 
-  pcl::PCLPointCloud2 cloud_;
+  sensor_msgs::msg::PointCloud2 cloud_;
   std::optional<OrganizedCloudMetadata> organized_metadata_;
 };
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp

@@ -26,6 +26,8 @@
 #include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
 #include <pcl/PCLPointCloud2.h>
 
 #include <algorithm>
@@ -340,10 +342,11 @@ class HesaiDecoder : public HesaiScanDecoder
 
   bool hasScanned() override { return has_scanned_; }
 
-  std::tuple<pcl::PCLPointCloud2Ptr, double> getPointcloud() override
+  std::tuple<sensor_msgs::msg::PointCloud2::UniquePtr, double> getPointcloud() override
   {
     double scan_timestamp_s = static_cast<double>(output_scan_timestamp_ns_) * 1e-9;
-    return {std::make_shared<pcl::PCLPointCloud2>(output_pc_->pop_cloud()), scan_timestamp_s};
+    return {
+      std::make_unique<sensor_msgs::msg::PointCloud2>(output_pc_->pop_cloud()), scan_timestamp_s};
   }
 };
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp

@@ -51,7 +51,7 @@ class HesaiScanDecoder
 
   /// @brief Returns the point cloud and timestamp of the last scan
   /// @return A tuple of point cloud and timestamp in nanoseconds
-  virtual std::tuple<pcl::PCLPointCloud2Ptr, double> getPointcloud() = 0;
+  virtual std::tuple<sensor_msgs::msg::PointCloud2::UniquePtr, double> getPointcloud() = 0;
 };
 }  // namespace nebula::drivers
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp

@@ -17,11 +17,9 @@
 
 #include "nebula_common/hesai/hesai_common.hpp"
 #include "nebula_common/nebula_status.hpp"
-#include "nebula_decoders/nebula_decoders_common/managed_point_cloud.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
 
-#include <pcl/PCLPointCloud2.h>
-#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <memory>
 #include <tuple>
@@ -70,7 +68,8 @@ class HesaiDriver
   /// @brief Convert raw packet to pointcloud
   /// @param packet Packet to convert
   /// @return Tuple of pointcloud and timestamp
-  std::tuple<pcl::PCLPointCloud2Ptr, double> ParseCloudPacket(const std::vector<uint8_t> & packet);
+  std::tuple<sensor_msgs::msg::PointCloud2::UniquePtr, double> ParseCloudPacket(
+    const std::vector<uint8_t> & packet);
 };
 
 }  // namespace drivers

nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp

@@ -13,8 +13,6 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp"
 
-#include <pcl/PCLPointCloud2.h>
-
 #include <memory>
 
 // #define WITH_DEBUG_STD_COUT_HESAI_CLIENT // Use std::cout messages for debugging
@@ -79,7 +77,7 @@ std::shared_ptr<HesaiScanDecoder> HesaiDriver::InitializeDecoder(
     sensor_configuration, std::dynamic_pointer_cast<const CalibT>(calibration_configuration));
 }
 
-std::tuple<pcl::PCLPointCloud2Ptr, double> HesaiDriver::ParseCloudPacket(
+std::tuple<sensor_msgs::msg::PointCloud2::UniquePtr, double> HesaiDriver::ParseCloudPacket(
   const std::vector<uint8_t> & packet)
 {
   if (driver_status_ != nebula::Status::OK) {

nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp

@@ -15,7 +15,7 @@
 #pragma once
 
 #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp"
-#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
+#include "nebula_ros/common/pointcloud_publishers.hpp"
 #include "nebula_ros/common/watchdog_timer.hpp"
 
 #include <nebula_common/hesai/hesai_common.hpp>
@@ -79,7 +79,7 @@ class HesaiDecoderWrapper
   rclcpp::Publisher<pandar_msgs::msg::PandarScan>::SharedPtr packets_pub_{};
   pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{};
 
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_{};
+  std::shared_ptr<PointCloudPublisher> nebula_points_pub_;
 
   std::shared_ptr<WatchdogTimer> cloud_watchdog_;
 };

nebula_ros/src/hesai/decoder_wrapper.cpp

@@ -2,15 +2,19 @@
 
 #include "nebula_ros/hesai/decoder_wrapper.hpp"
 
+#include "cuda_blackboard/cuda_blackboard_publisher.hpp"
+#include "cuda_blackboard/cuda_pointcloud2.hpp"
+#include "nebula_ros/common/pointcloud_publishers.hpp"
+
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/time.hpp>
 
-#include <pcl/PCLPointCloud2.h>
-#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <memory>
 #include <mutex>
+#include <utility>
 
 #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical"
 namespace nebula
@@ -63,8 +67,16 @@ HesaiDecoderWrapper::HesaiDecoderWrapper(
   auto pointcloud_qos =
     rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile);
 
-  nebula_points_pub_ =
-    parent_node->create_publisher<sensor_msgs::msg::PointCloud2>("pandar_points", pointcloud_qos);
+  if (config->publish_to_gpu) {
+    auto cuda_pub =
+      std::make_shared<cuda_blackboard::CudaBlackboardPublisher<cuda_blackboard::CudaPointCloud2>>(
+        parent_node_, "pandar_points");
+    nebula_points_pub_ = std::make_shared<CudaPointCloudPublisher>(cuda_pub);
+  } else {
+    auto pub =
+      parent_node_.create_publisher<sensor_msgs::msg::PointCloud2>("pandar_points", pointcloud_qos);
+    nebula_points_pub_ = std::make_shared<BasicPointCloudPublisher>(pub);
+  }
 
   RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_);
 
@@ -133,25 +145,13 @@ void HesaiDecoderWrapper::ProcessCloudPacket(
     current_scan_msg_ = std::make_unique<pandar_msgs::msg::PandarScan>();
   }
 
-  if (
-    nebula_points_pub_->get_subscription_count() > 0 ||
-    nebula_points_pub_->get_intra_process_subscription_count() > 0) {
-    auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
-    pcl_conversions::moveFromPCL(*pointcloud, *ros_pc_msg_ptr);
-    ros_pc_msg_ptr->header.stamp = rclcpp::Time(SecondsToChronoNanoSeconds(timestamp).count());
-    PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_);
-  }
-}
-
-void HesaiDecoderWrapper::PublishCloud(
-  std::unique_ptr<sensor_msgs::msg::PointCloud2> pointcloud,
-  const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & publisher)
-{
+  pointcloud->header.frame_id = sensor_cfg_->frame_id;
+  pointcloud->header.stamp = rclcpp::Time(SecondsToChronoNanoSeconds(timestamp).count());
   if (pointcloud->header.stamp.sec < 0) {
     RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source.");
   }
-  pointcloud->header.frame_id = sensor_cfg_->frame_id;
-  publisher->publish(std::move(pointcloud));
+
+  nebula_points_pub_->publish_if_subscribers_exist(std::move(pointcloud));
 }
 
 nebula::Status HesaiDecoderWrapper::Status()