feat: implemented a cuda accelerated pointcloud concatenation and int…

…egrated it with the cuda blackboard

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

sensing/autoware_pointcloud_preprocessor/CMakeLists.txt

@@ -13,11 +13,19 @@ find_package(Sophus REQUIRED)
 find_package(Boost REQUIRED)
 find_package(PCL REQUIRED)
 find_package(CGAL REQUIRED COMPONENTS Core)
+find_package(CUDA)
+
+if(${CUDA_FOUND})
+  add_definitions(-DUSE_CUDA)
+else()
+  message("CUDA wat not found. Disabling CUDA-dependent functionalities")
+endif()
 
 include_directories(
   include
   SYSTEM
   ${Boost_INCLUDE_DIRS}
+  ${CUDA_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
   ${Sophus_INCLUDE_DIRS}
@@ -63,6 +71,14 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter
   sensor_msgs
 )
 
+cuda_add_library(pointcloud_preprocessor_filter_cuda SHARED
+  src/concatenate_data/combine_cloud_handler_kernel.cu
+)
+
+target_include_directories(pointcloud_preprocessor_filter_cuda PUBLIC
+  ${cuda_blackboard_INCLUDE_DIRS}
+)
+
 ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/concatenate_data/concatenate_and_time_sync_node.cpp
   src/concatenate_data/combine_cloud_handler.cpp
@@ -93,8 +109,10 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
 
 target_link_libraries(pointcloud_preprocessor_filter
   pointcloud_preprocessor_filter_base
+  pointcloud_preprocessor_filter_cuda
   faster_voxel_grid_downsample_filter
   ${Boost_LIBRARIES}
+  ${CUDA_LIBRARIES}
   ${OpenCV_LIBRARIES}
   ${Sophus_LIBRARIES}
   ${PCL_LIBRARIES}

sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml

@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    use_cuda: false
     debug_mode: false
     has_static_tf_only: false
     rosbag_replay: false

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp

@@ -25,33 +25,36 @@ namespace autoware::pointcloud_preprocessor
 {
 
 class PointCloudConcatenateDataSynchronizerComponent;
+
+template <typename PointCloudMessage>
 class CombineCloudHandler;
 
+template <typename PointCloudMessage>
 class CloudCollector
 {
 public:
   CloudCollector(
     std::shared_ptr<PointCloudConcatenateDataSynchronizerComponent> && ros2_parent_node,
-    std::shared_ptr<CombineCloudHandler> & combine_cloud_handler, int num_of_clouds,
-    double timeout_sec, bool debug_mode);
+    std::shared_ptr<CombineCloudHandler<PointCloudMessage>> & combine_cloud_handler,
+    int num_of_clouds, double timeout_sec, bool debug_mode);
 
   void set_reference_timestamp(double timestamp, double noise_window);
   std::tuple<double, double> get_reference_timestamp_boundary();
   void process_pointcloud(
-    const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud);
+    const std::string & topic_name, typename PointCloudMessage::ConstSharedPtr cloud);
   void concatenate_callback();
 
-  ConcatenatedCloudResult concatenate_pointclouds(
-    std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> topic_to_cloud_map);
+  ConcatenatedCloudResult<PointCloudMessage> concatenate_pointclouds(
+    std::unordered_map<std::string, typename PointCloudMessage::ConstSharedPtr> topic_to_cloud_map);
 
-  std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr>
+  std::unordered_map<std::string, typename PointCloudMessage::ConstSharedPtr>
   get_topic_to_cloud_map();
 
 private:
   std::shared_ptr<PointCloudConcatenateDataSynchronizerComponent> ros2_parent_node_;
-  std::shared_ptr<CombineCloudHandler> combine_cloud_handler_;
+  std::shared_ptr<CombineCloudHandler<PointCloudMessage>> combine_cloud_handler_;
   rclcpp::TimerBase::SharedPtr timer_;
-  std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> topic_to_cloud_map_;
+  std::unordered_map<std::string, typename PointCloudMessage::ConstSharedPtr> topic_to_cloud_map_;
   uint64_t num_of_clouds_;
   double timeout_sec_;
   double reference_timestamp_min_{0.0};

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp

@@ -16,6 +16,7 @@
 
 #include <deque>
 #include <memory>
+#include <mutex>
 #include <optional>
 #include <string>
 #include <unordered_map>
@@ -43,22 +44,41 @@
 #include <message_filters/sync_policies/exact_time.h>
 #include <message_filters/synchronizer.h>
 
+#ifdef USE_CUDA
+#include <cuda_blackboard/cuda_pointcloud2.hpp>
+#endif
+
 namespace autoware::pointcloud_preprocessor
 {
 using autoware::point_types::PointXYZIRC;
 using point_cloud_msg_wrapper::PointCloud2Modifier;
 
-struct ConcatenatedCloudResult
+template <typename PointCloudMessage>
+struct ConcatenatedCloudResult;
+
+template <>
+struct ConcatenatedCloudResult<sensor_msgs::msg::PointCloud2>
 {
   sensor_msgs::msg::PointCloud2::SharedPtr concatenate_cloud_ptr{nullptr};
   std::optional<std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr>>
     topic_to_transformed_cloud_map;
   std::unordered_map<std::string, double> topic_to_original_stamp_map;
 };
 
-class CombineCloudHandler
+#ifdef USE_CUDA
+template <>
+struct ConcatenatedCloudResult<cuda_blackboard::CudaPointCloud2>
+{
+  cuda_blackboard::CudaPointCloud2::UniquePtr concatenate_cloud_ptr{nullptr};
+  std::optional<std::unordered_map<std::string, cuda_blackboard::CudaPointCloud2::UniquePtr>>
+    topic_to_transformed_cloud_map;
+  std::unordered_map<std::string, double> topic_to_original_stamp_map;
+};
+#endif
+
+class CombineCloudHandlerBase
 {
-private:
+protected:
   rclcpp::Node & node_;
 
   std::vector<std::string> input_topics_;
@@ -80,9 +100,41 @@ class CombineCloudHandler
     }
   };
 
+public:
+  CombineCloudHandlerBase(
+    rclcpp::Node & node, std::vector<std::string> input_topics, std::string output_frame,
+    bool is_motion_compensated, bool publish_synchronized_pointcloud,
+    bool keep_input_frame_in_synchronized_pointcloud, bool has_static_tf_only);
+
+  virtual ~CombineCloudHandlerBase() = default;
+
+  void process_twist(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg);
+
+  void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & input);
+
+  Eigen::Matrix4f compute_transform_to_adjust_for_old_timestamp(
+    const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp);
+
+  std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
+};
+
+template <typename PointCloudMessage>
+class CombineCloudHandler : public CombineCloudHandlerBase
+{
+public:
+  ConcatenatedCloudResult<PointCloudMessage> combine_pointclouds(
+    std::unordered_map<std::string, typename PointCloudMessage::ConstSharedPtr> &
+      topic_to_cloud_map);
+};
+
+template <>
+class CombineCloudHandler<sensor_msgs::msg::PointCloud2> : public CombineCloudHandlerBase
+{
+protected:
   static void convert_to_xyzirc_cloud(
-    const sensor_msgs::msg::PointCloud2::SharedPtr & input_cloud,
-    sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud);
+    const typename sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud,
+    typename sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud);
 
   void correct_pointcloud_motion(
     const std::shared_ptr<sensor_msgs::msg::PointCloud2> & transformed_cloud_ptr,
@@ -95,16 +147,44 @@ class CombineCloudHandler
     rclcpp::Node & node, std::vector<std::string> input_topics, std::string output_frame,
     bool is_motion_compensated, bool publish_synchronized_pointcloud,
     bool keep_input_frame_in_synchronized_pointcloud, bool has_static_tf_only);
-  void process_twist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & input);
-  void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & input);
 
-  ConcatenatedCloudResult combine_pointclouds(
-    std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> & topic_to_cloud_map);
+  ConcatenatedCloudResult<sensor_msgs::msg::PointCloud2> combine_pointclouds(
+    std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::ConstSharedPtr> &
+      topic_to_cloud_map);
+};
 
-  Eigen::Matrix4f compute_transform_to_adjust_for_old_timestamp(
-    const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp);
+#ifdef USE_CUDA
 
-  std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
+template <>
+class CombineCloudHandler<cuda_blackboard::CudaPointCloud2> : public CombineCloudHandlerBase
+{
+protected:
+  struct CudaConcatStruct
+  {
+    cudaStream_t stream;
+    std::unique_ptr<cuda_blackboard::CudaPointCloud2> cloud_ptr;
+    std::size_t max_pointcloud_size_{0};
+  };
+
+  std::unordered_map<std::string, CudaConcatStruct> cuda_concat_struct_map_;
+  std::unique_ptr<cuda_blackboard::CudaPointCloud2> concatenated_cloud_ptr;
+  std::size_t max_concat_pointcloud_size_{0};
+  std::mutex mutex_;
+
+public:
+  CombineCloudHandler(
+    rclcpp::Node & node, std::vector<std::string> input_topics, std::string output_frame,
+    bool is_motion_compensated, bool publish_synchronized_pointcloud,
+    bool keep_input_frame_in_synchronized_pointcloud, bool has_static_tf_only);
+
+  void allocate_pointclouds();
+
+public:
+  ConcatenatedCloudResult<cuda_blackboard::CudaPointCloud2> combine_pointclouds(
+    std::unordered_map<std::string, cuda_blackboard::CudaPointCloud2::ConstSharedPtr> &
+      topic_to_cloud_map);
 };
 
+#endif
+
 }  // namespace autoware::pointcloud_preprocessor

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler_kernel.hpp

@@ -0,0 +1,61 @@
+// Copyright 2024 TIER IV, 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.
+
+#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__COMBINE_CLOUD_HANDLER_KERNEL_HPP_
+#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__COMBINE_CLOUD_HANDLER_KERNEL_HPP_
+
+#ifdef USE_CUDA
+#include <cuda_runtime.h>
+#endif
+
+#include <cstdint>
+
+namespace autoware::pointcloud_preprocessor
+{
+
+struct TransformStruct
+{
+  float translation_x;
+  float translation_y;
+  float translation_z;
+  float m11;
+  float m12;
+  float m13;
+  float m21;
+  float m22;
+  float m23;
+  float m31;
+  float m32;
+  float m33;
+};
+
+struct PointTypeStruct
+{
+  float x;
+  float y;
+  float z;
+  std::uint8_t intensity;
+  std::uint8_t return_type;
+  std::uint16_t channel;
+};
+
+#ifdef USE_CUDA
+void transform_launch(
+  const PointTypeStruct * input_points, int num_points, TransformStruct transform,
+  PointTypeStruct * output_points, cudaStream_t & stream);
+#endif
+
+}  // namespace autoware::pointcloud_preprocessor
+
+#endif  // AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__COMBINE_CLOUD_HANDLER_KERNEL_HPP_