feat(autoware_pointcloud_preprocessor): cuda-accelerated pointcloud concatenation

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,8 +71,18 @@ 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_nodelet.cpp
+  src/concatenate_data/concatenate_and_time_sync_node.cpp
+  src/concatenate_data/combine_cloud_handler.cpp
+  src/concatenate_data/cloud_collector.cpp
   src/concatenate_data/concatenate_pointclouds.cpp
   src/crop_box_filter/crop_box_filter_node.cpp
   src/time_synchronizer/time_synchronizer_node.cpp
@@ -91,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}
@@ -111,7 +131,7 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
 # ========== Concatenate and Sync data ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent"
-  EXECUTABLE concatenate_data_node)
+  EXECUTABLE concatenate_and_time_sync_node)
 
 # ========== CropBox ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
@@ -243,8 +263,17 @@ if(BUILD_TESTING)
     test/test_distortion_corrector_node.cpp
   )
 
+  ament_add_gtest(test_concatenate_node_unit
+    test/test_concatenate_node_unit.cpp
+  )
+
   target_link_libraries(test_utilities pointcloud_preprocessor_filter)
   target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter)
+  target_link_libraries(test_concatenate_node_unit pointcloud_preprocessor_filter)
 
+  add_ros_test(
+    test/test_concatenate_node_component.py
+    TIMEOUT "50"
+  )
 
 endif()

sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml

@@ -0,0 +1,23 @@
+/**:
+  ros__parameters:
+    use_cuda: false
+    debug_mode: false
+    has_static_tf_only: false
+    rosbag_replay: false
+    rosbag_length: 20.0
+    maximum_queue_size: 5
+    timeout_sec: 0.2
+    is_motion_compensated: true
+    publish_synchronized_pointcloud: true
+    keep_input_frame_in_synchronized_pointcloud: true
+    publish_previous_but_late_pointcloud: false
+    synchronized_pointcloud_postfix: pointcloud
+    input_twist_topic_type: twist
+    input_topics: [
+                    "/sensing/lidar/right/pointcloud_before_sync",
+                    "/sensing/lidar/top/pointcloud_before_sync",
+                    "/sensing/lidar/left/pointcloud_before_sync",
+                ]
+    output_frame: base_link
+    lidar_timestamp_offsets: [0.0, 0.015, 0.016]
+    lidar_timestamp_noise_window: [0.01, 0.01, 0.01]

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

@@ -0,0 +1,65 @@
+// 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.
+
+#pragma once
+
+#include "combine_cloud_handler.hpp"
+
+#include <memory>
+#include <string>
+#include <tuple>
+#include <unordered_map>
+
+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<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, typename PointCloudMessage::ConstSharedPtr cloud);
+  void concatenate_callback();
+
+  ConcatenatedCloudResult<PointCloudMessage> concatenate_pointclouds(
+    std::unordered_map<std::string, typename PointCloudMessage::ConstSharedPtr> topic_to_cloud_map);
+
+  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<PointCloudMessage>> combine_cloud_handler_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  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};
+  double reference_timestamp_max_{0.0};
+  bool debug_mode_;
+};
+
+}  // namespace autoware::pointcloud_preprocessor

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

@@ -0,0 +1,190 @@
+// 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.
+
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+// ROS includes
+#include "autoware/point_types/types.hpp"
+
+#include <autoware/universe_utils/ros/managed_transform_buffer.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <tier4_debug_msgs/msg/int32_stamped.hpp>
+#include <tier4_debug_msgs/msg/string_stamped.hpp>
+
+#include <message_filters/pass_through.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#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;
+
+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;
+};
+
+#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
+{
+protected:
+  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_;
+  std::unique_ptr<autoware::universe_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
+
+  std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
+
+  /// @brief RclcppTimeHash structure defines a custom hash function for the rclcpp::Time type by
+  /// using its nanoseconds representation as the hash value.
+  struct RclcppTimeHash
+  {
+    std::size_t operator()(const rclcpp::Time & t) const
+    {
+      return std::hash<int64_t>()(t.nanoseconds());
+    }
+  };
+
+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 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,
+    const std::vector<rclcpp::Time> & pc_stamps,
+    std::unordered_map<rclcpp::Time, Eigen::Matrix4f, RclcppTimeHash> & transform_memo,
+    std::shared_ptr<sensor_msgs::msg::PointCloud2> transformed_delay_compensated_cloud_ptr);
+
+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);
+
+  ConcatenatedCloudResult<sensor_msgs::msg::PointCloud2> combine_pointclouds(
+    std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::ConstSharedPtr> &
+      topic_to_cloud_map);
+};
+
+#ifdef USE_CUDA
+
+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