feat(autoware_pointcloud_preprocessor): cuda-accelerated pointcloud concatenation

sensing/autoware_cuda_pointcloud_preprocessor/CMakeLists.txt

@@ -0,0 +1,94 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_cuda_pointcloud_preprocessor)
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(CUDA)
+
+if(NOT ${CUDA_FOUND})
+  message(WARNING "cuda was not found, so the autoware_cuda_pointcloud_preprocessor package will not be built.")
+  return()
+endif()
+
+ament_auto_find_build_dependencies()
+
+# Default to C++17
+if (NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif ()
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function)
+endif ()
+
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+endif()
+
+include_directories(
+    include
+    SYSTEM
+    ${CUDA_INCLUDE_DIRS}
+)
+
+list(APPEND CUDA_NVCC_FLAGS "--expt-relaxed-constexpr -diag-suppress 20012") # cSpell: ignore expt
+
+cuda_add_library(cuda_pointcloud_preprocessor_lib SHARED
+  src/cuda_concatenate_data/cuda_combine_cloud_handler.cpp
+  src/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.cu
+  src/cuda_concatenate_data/cuda_cloud_collector.cpp
+  src/cuda_concatenate_data/cuda_collector_matching_strategy.cpp
+)
+
+target_link_libraries(cuda_pointcloud_preprocessor_lib
+  ${autoware_pointcloud_preprocessor_TARGETS}
+)
+
+target_include_directories(cuda_pointcloud_preprocessor_lib SYSTEM PRIVATE
+  ${autoware_pointcloud_preprocessor_INCLUDE_DIRS}
+  ${autoware_point_types_INCLUDE_DIRS}
+  ${cuda_blackboard_INCLUDE_DIRS}
+  ${diagnostic_msgs_INCLUDE_DIRS}
+  ${geometry_msgs_INCLUDE_DIRS}
+  ${rclcpp_INCLUDE_DIRS}
+  ${rclcpp_components_INCLUDE_DIRS}
+  ${rcl_interfaces_INCLUDE_DIRS}
+  ${sensor_msgs_INCLUDE_DIRS}
+  ${tf2_INCLUDE_DIRS}
+  ${tf2_msgs_INCLUDE_DIRS}
+)
+
+# Targets
+ament_auto_add_library(cuda_pointcloud_preprocessor SHARED
+  src/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.cpp
+)
+
+target_link_libraries(cuda_pointcloud_preprocessor
+  ${CUDA_LIBRARIES}
+
+  ${diagnostic_msgs_LIBRARIES}
+  cuda_pointcloud_preprocessor_lib
+)
+
+# ========== Concatenate and Sync data ==========
+rclcpp_components_register_node(cuda_pointcloud_preprocessor
+  PLUGIN "autoware::cuda_pointcloud_preprocessor::CudaPointCloudConcatenateDataSynchronizerComponent"
+  EXECUTABLE cuda_concatenate_and_time_sync_node)
+
+install(DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_auto_package()
+
+# Set ROS_DISTRO macros
+set(ROS_DISTRO $ENV{ROS_DISTRO})
+if(${ROS_DISTRO} STREQUAL "rolling")
+    add_compile_definitions(ROS_DISTRO_ROLLING)
+elseif(${ROS_DISTRO} STREQUAL "foxy")
+    add_compile_definitions(ROS_DISTRO_FOXY)
+elseif(${ROS_DISTRO} STREQUAL "galactic")
+    add_compile_definitions(ROS_DISTRO_GALACTIC)
+elseif(${ROS_DISTRO} STREQUAL "humble")
+    add_compile_definitions(ROS_DISTRO_HUMBLE)
+endif()

sensing/autoware_cuda_pointcloud_preprocessor/README.md

@@ -0,0 +1,20 @@
+# autoware_cuda_pointcloud_preprocessor
+
+## Purpose
+
+The pointcloud preprocessing implemented in `autoware_pointcloud_preprocessor` has been thoroughly tested in autoware. However, the latency it introduces does not scale well with modern LiDAR devices due to the high number of points they introduce.
+
+To alleviate this issue, this package reimplements most of the pipeline presented in `autoware_pointcloud_preprocessor` leveraging the use of GPGPUs. In particular, this package makes use of CUDA to provide accelerated versions of the already established implementations, while also maintaining compatibility with normal ROS nodes/topics. <!-- cSpell: ignore GPGPUs >
+
+## Inner-workings / Algorithms
+
+A detailed description of each filter's algorithm is available in the following links.
+
+| Filter Name                         | Description                                                                                                                                  | Detail                                       |
+| ----------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------- |
+| cuda_pointcloud_preprocessor        | Implements the cropping, distortion correction, and outlier filtering (ring-based) of the `autoware_pointcloud_preprocessor`'s CPU versions. | [link](docs/cuda-pointcloud-preprocessor.md) |
+| cuda_concatenate_and_time_sync_node | Implements pointcloud concatenation an synchronization following `autoware_pointcloud_preprocessor`'s CPU implementation.                    | [link](docs/cuda-concatenate-data.md)        |
+
+## (Optional) Future extensions / Unimplemented parts
+
+The subsample filters implemented in `autoware_pointcloud_preprocessor` will have similar counterparts in this package.

sensing/autoware_cuda_pointcloud_preprocessor/config/cuda_concatenate_and_time_sync_node.param.yaml

@@ -0,0 +1,21 @@
+/**:
+  ros__parameters:
+    debug_mode: false
+    has_static_tf_only: false
+    rosbag_length: 10.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
+    matching_strategy:
+      type: naive

sensing/autoware_cuda_pointcloud_preprocessor/docs/cuda-concatenate-data.md

@@ -0,0 +1,6 @@
+# cuda_concatenate_and_time_synchronize_node
+
+This package is a cuda accelerated version of the one available in [autoware_cuda_pointcloud_preprocessor](../autoware_pointcloud_preprocessor).
+As this node is templated, the overall design, algorithm, inputs, and outputs are the same.
+
+The only change, corresponds to the pointcloud topics, which instead of using the standard `sensor_msgs::msg::PointCloud2` message type, they use the `cuda_blackboard` mechanism.

sensing/autoware_cuda_pointcloud_preprocessor/include/autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_cloud_collector.hpp

@@ -0,0 +1,18 @@
+// Copyright 2025 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 "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp"
+#include "cuda_combine_cloud_handler.hpp"

sensing/autoware_cuda_pointcloud_preprocessor/include/autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_combine_cloud_handler.hpp

@@ -0,0 +1,53 @@
+// Copyright 2025 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 "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp"
+#include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp"
+
+namespace autoware::pointcloud_preprocessor
+{
+
+template <>
+class CombineCloudHandler<CudaPointCloud2Traits> : public CombineCloudHandlerBase
+{
+protected:
+  struct CudaConcatStruct
+  {
+    cudaStream_t stream;
+    std::unique_ptr<CudaPointCloud2Traits::PointCloudMessage> cloud_ptr;
+    std::size_t max_pointcloud_size{0};
+  };
+
+  std::unordered_map<std::string, CudaConcatStruct> cuda_concat_struct_map_;
+  std::unique_ptr<CudaPointCloud2Traits::PointCloudMessage> concatenated_cloud_ptr_;
+  std::size_t max_concat_pointcloud_size_{0};
+  std::mutex mutex_;
+
+public:
+  CombineCloudHandler(
+    rclcpp::Node & node, const 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<CudaPointCloud2Traits> combine_pointclouds(
+    std::unordered_map<
+      std::string, typename CudaPointCloud2Traits::PointCloudMessage::ConstSharedPtr> &
+      topic_to_cloud_map);
+
+  void allocate_pointclouds() override;
+};
+
+}  // namespace autoware::pointcloud_preprocessor

sensing/autoware_cuda_pointcloud_preprocessor/include/autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.hpp

@@ -0,0 +1,57 @@
+// Copyright 2025 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__CUDA_POINTCLOUD_PREPROCESSOR__CUDA_CONCATENATE_DATA__CUDA_COMBINE_CLOUD_HANDLER_KERNEL_HPP_
+#define AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__CUDA_CONCATENATE_DATA__CUDA_COMBINE_CLOUD_HANDLER_KERNEL_HPP_
+
+#include <cuda_runtime.h>
+
+#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;
+};
+
+void transform_launch(
+  const PointTypeStruct * input_points, int num_points, TransformStruct transform,
+  PointTypeStruct * output_points, cudaStream_t & stream);
+
+}  // namespace autoware::pointcloud_preprocessor
+
+#endif  // AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__CUDA_CONCATENATE_DATA__CUDA_COMBINE_CLOUD_HANDLER_KERNEL_HPP_

sensing/autoware_cuda_pointcloud_preprocessor/include/autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.hpp

@@ -0,0 +1,42 @@
+// Copyright 2025 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 "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp"
+#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp"
+
+#include <cuda_blackboard/cuda_blackboard_publisher.hpp>
+#include <cuda_blackboard/cuda_blackboard_subscriber.hpp>
+#include <cuda_blackboard/cuda_pointcloud2.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace autoware::cuda_pointcloud_preprocessor
+{
+
+class CudaPointCloudConcatenateDataSynchronizerComponent
+: public autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponentTemplated<
+    autoware::pointcloud_preprocessor::CudaPointCloud2Traits>
+{
+public:
+  explicit CudaPointCloudConcatenateDataSynchronizerComponent(
+    const rclcpp::NodeOptions & node_options)
+  : autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponentTemplated<
+      autoware::pointcloud_preprocessor::CudaPointCloud2Traits>(node_options)
+  {
+  }
+  ~CudaPointCloudConcatenateDataSynchronizerComponent() override = default;
+};
+
+}  // namespace autoware::cuda_pointcloud_preprocessor

sensing/autoware_cuda_pointcloud_preprocessor/include/autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp

@@ -0,0 +1,31 @@
+// Copyright 2025 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 <cuda_blackboard/cuda_blackboard_publisher.hpp>
+#include <cuda_blackboard/cuda_blackboard_subscriber.hpp>
+#include <cuda_blackboard/cuda_pointcloud2.hpp>
+
+namespace autoware::pointcloud_preprocessor
+{
+
+struct CudaPointCloud2Traits
+{
+  using PointCloudMessage = cuda_blackboard::CudaPointCloud2;
+  using PublisherType = cuda_blackboard::CudaBlackboardPublisher<PointCloudMessage>;
+  using SubscriberType = cuda_blackboard::CudaBlackboardSubscriber<PointCloudMessage>;
+};
+
+}  // namespace autoware::pointcloud_preprocessor

sensing/autoware_cuda_pointcloud_preprocessor/launch/cuda_concatenate_and_time_sync_node.launch.xml

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+  <arg name="output" default="/sensing/lidar/concatenated/pointcloud"/>
+  <!-- Parameter -->
+  <arg name="param_file" default="$(find-pkg-share autoware_cuda_pointcloud_preprocessor)/config/cuda_concatenate_and_time_sync_node.param.yaml"/>
+  <node pkg="autoware_cuda_pointcloud_preprocessor" exec="cuda_concatenate_and_time_sync_node" name="cuda_concatenate_and_time_sync_node" output="screen">
+    <remap from="~/input/twist" to="$(var input/twist)"/>
+    <remap from="output" to="$(var output)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

sensing/autoware_cuda_pointcloud_preprocessor/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>autoware_cuda_pointcloud_preprocessor</name>
+  <version>0.1.0</version>
+  <description>autoware_cuda_pointcloud_preprocessor</description>
+  <maintainer email="[email protected]">Kenzo Lobos-Tsunekawa</maintainer>
+  <author email="[email protected]">Kenzo Lobos-Tsunekawa</author>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_point_types</depend>
+  <depend>autoware_pointcloud_preprocessor</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>cuda_blackboard</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tier4_debug_msgs</depend>
+
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>