feat(autoware_lidar_centerpoint): added the cuda_blackboard to centerpoint

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp

@@ -20,6 +20,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 namespace autoware::image_projection_based_fusion
 {
@@ -34,10 +35,13 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT
     const autoware::lidar_centerpoint::DensificationParam & densification_param,
     const autoware::lidar_centerpoint::CenterPointConfig & config);
 
+  bool detect(
+    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
+    std::vector<autoware::lidar_centerpoint::Box3D> & det_boxes3d);
+
 protected:
   bool preprocess(
-    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg,
-    const tf2_ros::Buffer & tf_buffer) override;
+    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer);
 
   std::unique_ptr<image_projection_based_fusion::VoxelGenerator> vg_ptr_pp_{nullptr};
 };

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp

@@ -38,6 +38,24 @@ PointPaintingTRT::PointPaintingTRT(
     std::make_unique<image_projection_based_fusion::VoxelGenerator>(densification_param, config_);
 }
 
+bool PointPaintingTRT::detect(
+  const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
+  std::vector<autoware::lidar_centerpoint::Box3D> & det_boxes3d)
+{
+  CHECK_CUDA_ERROR(cudaMemsetAsync(
+    encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_));
+  CHECK_CUDA_ERROR(
+    cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_));
+  if (!preprocess(input_pointcloud_msg, tf_buffer)) {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect.");
+    return false;
+  }
+  inference();
+  postProcess(det_boxes3d);
+  return true;
+}
+
 bool PointPaintingTRT::preprocess(
   const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
 {

perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp

@@ -19,10 +19,11 @@
 #include "autoware/lidar_centerpoint/network/network_trt.hpp"
 #include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp"
 #include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp"
-#include "pcl/point_cloud.h"
-#include "pcl/point_types.h"
 
-#include "sensor_msgs/msg/point_cloud2.hpp"
+#include <cuda_blackboard/cuda_pointcloud2.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
 #include <memory>
 #include <string>
@@ -61,14 +62,15 @@ class CenterPointTRT
   virtual ~CenterPointTRT();
 
   bool detect(
-    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
-    std::vector<Box3D> & det_boxes3d);
+    const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
+    const tf2_ros::Buffer & tf_buffer, std::vector<Box3D> & det_boxes3d);
 
 protected:
   void initPtr();
 
   virtual bool preprocess(
-    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer);
+    const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
+    const tf2_ros::Buffer & tf_buffer);
 
   void inference();
 

perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp

@@ -22,6 +22,10 @@
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
+#include <cuda_blackboard/cuda_adaptation.hpp>
+#include <cuda_blackboard/cuda_blackboard_subscriber.hpp>
+#include <cuda_blackboard/cuda_pointcloud2.hpp>
+#include <cuda_blackboard/negotiated_types.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
@@ -43,12 +47,14 @@ class LidarCenterPointNode : public rclcpp::Node
   explicit LidarCenterPointNode(const rclcpp::NodeOptions & node_options);
 
 private:
-  void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg);
+  void pointCloudCallback(
+    const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg);
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
 
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
+  std::unique_ptr<cuda_blackboard::CudaBlackboardSubscriber<cuda_blackboard::CudaPointCloud2>>
+    pointcloud_sub_;
   rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_;
 
   std::vector<std::string> class_names_;

perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp

@@ -15,19 +15,21 @@
 #ifndef AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_
 #define AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_
 
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
+#include <cuda_blackboard/cuda_pointcloud2.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <list>
 #include <string>
 #include <utility>
 #ifdef ROS_DISTRO_GALACTIC
-#include "tf2_sensor_msgs/tf2_sensor_msgs.h"
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
 #else
-#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 #endif
 
-#include "autoware/lidar_centerpoint/cuda_utils.hpp"
+#include <memory>
 
 namespace autoware::lidar_centerpoint
 {
@@ -50,10 +52,7 @@ class DensificationParam
 
 struct PointCloudWithTransform
 {
-  cuda::unique_ptr<float[]> points_d{nullptr};
-  std_msgs::msg::Header header;
-  std::size_t num_points{0};
-  std::size_t point_step{0};
+  std::shared_ptr<const cuda_blackboard::CudaPointCloud2> input_pointcloud_msg_ptr;
   Eigen::Affine3f affine_past2world;
 };
 
@@ -63,8 +62,8 @@ class PointCloudDensification
   explicit PointCloudDensification(const DensificationParam & param);
 
   bool enqueuePointCloud(
-    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
-    cudaStream_t stream);
+    const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
+    const tf2_ros::Buffer & tf_buffer);
 
   double getCurrentTimestamp() const { return current_timestamp_; }
   Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; }
@@ -80,7 +79,8 @@ class PointCloudDensification
 
 private:
   void enqueue(
-    const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine, cudaStream_t stream);
+    const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
+    const Eigen::Affine3f & affine);
   void dequeue();
 
   DensificationParam param_;

perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp

@@ -18,7 +18,7 @@
 #include "autoware/lidar_centerpoint/centerpoint_config.hpp"
 #include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp"
 
-#include "sensor_msgs/msg/point_cloud2.hpp"
+#include <cuda_blackboard/cuda_pointcloud2.hpp>
 
 #include <memory>
 #include <vector>
@@ -35,8 +35,8 @@ class VoxelGeneratorTemplate
   virtual std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) = 0;
 
   bool enqueuePointCloud(
-    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
-    cudaStream_t stream);
+    const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
+    const tf2_ros::Buffer & tf_buffer);
 
 protected:
   std::unique_ptr<PointCloudDensification> pd_ptr_{nullptr};

perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml

@@ -17,6 +17,7 @@
     <load_composable_node target="$(var pointcloud_container_name)">
       <composable_node pkg="autoware_lidar_centerpoint" plugin="autoware::lidar_centerpoint::LidarCenterPointNode" name="lidar_centerpoint">
         <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
+        <remap from="~/input/pointcloud/cuda" to="$(var input/pointcloud)/cuda"/>
         <remap from="~/output/objects" to="$(var output/objects)"/>
         <param from="$(var model_param_path)" allow_substs="true"/>
         <param from="$(var ml_package_param_path)" allow_substs="true"/>

perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp

@@ -126,15 +126,15 @@ void CenterPointTRT::initPtr()
 }
 
 bool CenterPointTRT::detect(
-  const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
-  std::vector<Box3D> & det_boxes3d)
+  const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
+  const tf2_ros::Buffer & tf_buffer, std::vector<Box3D> & det_boxes3d)
 {
   CHECK_CUDA_ERROR(cudaMemsetAsync(
     encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_));
   CHECK_CUDA_ERROR(
     cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_));
 
-  if (!preprocess(input_pointcloud_msg, tf_buffer)) {
+  if (!preprocess(input_pointcloud_msg_ptr, tf_buffer)) {
     RCLCPP_WARN(rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect.");
     return false;
   }
@@ -161,9 +161,10 @@ bool CenterPointTRT::detect(
 }
 
 bool CenterPointTRT::preprocess(
-  const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
+  const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
+  const tf2_ros::Buffer & tf_buffer)
 {
-  bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream_);
+  bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg_ptr, tf_buffer);
   if (!is_success) {
     return false;
   }

perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp

@@ -60,10 +60,10 @@ PointCloudDensification::PointCloudDensification(const DensificationParam & para
 }
 
 bool PointCloudDensification::enqueuePointCloud(
-  const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
-  cudaStream_t stream)
+  const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & pointcloud_msg_ptr,
+  const tf2_ros::Buffer & tf_buffer)
 {
-  const auto header = pointcloud_msg.header;
+  const auto header = pointcloud_msg_ptr->header;
 
   if (param_.pointcloud_cache_size() > 1) {
     auto transform_world2current =
@@ -73,9 +73,9 @@ bool PointCloudDensification::enqueuePointCloud(
     }
     auto affine_world2current = transformToEigen(transform_world2current.get());
 
-    enqueue(pointcloud_msg, affine_world2current, stream);
+    enqueue(pointcloud_msg_ptr, affine_world2current);
   } else {
-    enqueue(pointcloud_msg, Eigen::Affine3f::Identity(), stream);
+    enqueue(pointcloud_msg_ptr, Eigen::Affine3f::Identity());
   }
 
   dequeue();
@@ -84,24 +84,13 @@ bool PointCloudDensification::enqueuePointCloud(
 }
 
 void PointCloudDensification::enqueue(
-  const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current,
-  cudaStream_t stream)
+  const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & msg_ptr,
+  const Eigen::Affine3f & affine_world2current)
 {
   affine_world2current_ = affine_world2current;
-  current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds();
-
-  assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 0);
-  auto points_d = cuda::make_unique<float[]>(
-    sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float));
-  CHECK_CUDA_ERROR(cudaMemcpyAsync(
-    points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step,
-    cudaMemcpyHostToDevice, stream));
-
-  PointCloudWithTransform pointcloud = {
-    std::move(points_d), msg.header, msg.width * msg.height, msg.point_step,
-    affine_world2current.inverse()};
-
-  pointcloud_cache_.push_front(std::move(pointcloud));
+  current_timestamp_ = rclcpp::Time(msg_ptr->header.stamp).seconds();
+  PointCloudWithTransform pointcloud = {msg_ptr, affine_world2current.inverse()};
+  pointcloud_cache_.push_front(pointcloud);
 }
 
 void PointCloudDensification::dequeue()

perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp

@@ -44,23 +44,25 @@ VoxelGeneratorTemplate::VoxelGeneratorTemplate(
 }
 
 bool VoxelGeneratorTemplate::enqueuePointCloud(
-  const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
-  cudaStream_t stream)
+  const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
+  const tf2_ros::Buffer & tf_buffer)
 {
-  return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream);
+  return pd_ptr_->enqueuePointCloud(input_pointcloud_msg_ptr, tf_buffer);
 }
 
 std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t stream)
 {
   std::size_t point_counter = 0;
   for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter);
        pc_cache_iter++) {
-    auto sweep_num_points = pc_cache_iter->num_points;
-    auto point_step = pc_cache_iter->point_step;
+    const auto & input_pointcloud_msg_ptr = pc_cache_iter->input_pointcloud_msg_ptr;
+    auto sweep_num_points = input_pointcloud_msg_ptr->height * input_pointcloud_msg_ptr->width;
+    auto point_step = input_pointcloud_msg_ptr->point_step;
     auto affine_past2current =
       pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world;
     float time_lag = static_cast<float>(
-      pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds());
+      pd_ptr_->getCurrentTimestamp() -
+      rclcpp::Time(input_pointcloud_msg_ptr->header.stamp).seconds());
 
     if (point_counter + sweep_num_points > config_.cloud_capacity_) {
       RCLCPP_WARN_STREAM(
@@ -73,9 +75,9 @@ std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t s
     static_assert(std::is_same<decltype(affine_past2current.matrix()), Eigen::Matrix4f &>::value);
     static_assert(!Eigen::Matrix4f::IsRowMajor, "matrices should be col-major.");
     generateSweepPoints_launch(
-      pc_cache_iter->points_d.get(), sweep_num_points, point_step / sizeof(float), time_lag,
-      affine_past2current.matrix().data(), config_.point_feature_size_,
-      points_d + config_.point_feature_size_ * point_counter, stream);
+      reinterpret_cast<float *>(input_pointcloud_msg_ptr->data.get()), sweep_num_points,
+      point_step / sizeof(float), time_lag, affine_past2current.matrix().data(),
+      config_.point_feature_size_, points_d + config_.point_feature_size_ * point_counter, stream);
 
     point_counter += sweep_num_points;
   }

perception/autoware_lidar_centerpoint/package.xml

@@ -16,6 +16,7 @@
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_tensorrt_common</depend>
   <depend>autoware_universe_utils</depend>
+  <depend>cuda_blackboard</depend>
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

perception/autoware_lidar_centerpoint/src/node.cpp

@@ -108,9 +108,10 @@
   detector_ptr_ =
     std::make_unique<CenterPointTRT>(encoder_param, head_param, densification_param, config);
 
-  pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-    "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1),
-    std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1));
+  pointcloud_sub_ =
+    std::make_unique<cuda_blackboard::CudaBlackboardSubscriber<cuda_blackboard::CudaPointCloud2>>(
+      *this, "~/input/pointcloud", false,
+      std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1));
   objects_pub_ = this->create_publisher<autoware_perception_msgs::msg::DetectedObjects>(
     "~/output/objects", rclcpp::QoS{1});
 
@@ -133,7 +134,7 @@
 }
 
 void LidarCenterPointNode::pointCloudCallback(
-  const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg)
+  const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg)
 {
   const auto objects_sub_count =
     objects_pub_->get_subscription_count() + objects_pub_->get_intra_process_subscription_count();
@@ -146,7 +147,7 @@
   }
 
   std::vector<Box3D> det_boxes3d;
-  bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d);
+  bool is_success = detector_ptr_->detect(input_pointcloud_msg, tf_buffer_, det_boxes3d);
   if (!is_success) {
     return;
   }