feat(autoware_cuda_pointcloud_preprocessor): pointcloud concatenation

sensing/autoware_cuda_pointcloud_preprocessor/CMakeLists.txt

@@ -37,6 +37,10 @@ include_directories(
 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
   src/cuda_pointcloud_preprocessor/cuda_pointcloud_preprocessor.cu
   src/cuda_pointcloud_preprocessor/common_kernels.cu
   src/cuda_pointcloud_preprocessor/organize_kernels.cu
@@ -65,14 +69,23 @@ target_include_directories(cuda_pointcloud_preprocessor_lib SYSTEM PRIVATE
 
 # Targets
 ament_auto_add_library(cuda_pointcloud_preprocessor SHARED
+  src/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.cpp
   src/cuda_pointcloud_preprocessor/cuda_pointcloud_preprocessor_node.cpp
 )
 
 target_link_libraries(cuda_pointcloud_preprocessor
   ${CUDA_LIBRARIES}
+  ${diagnostic_msgs_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)
+
+# ========== Concatenate and Sync data ==========
 rclcpp_components_register_node(cuda_pointcloud_preprocessor
   PLUGIN "autoware::cuda_pointcloud_preprocessor::CudaPointcloudPreprocessorNode"
   EXECUTABLE cuda_pointcloud_preprocessor_node

sensing/autoware_cuda_pointcloud_preprocessor/README.md

@@ -10,9 +10,10 @@ To alleviate this issue, this package reimplements most of the pipeline presente
 
 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) |
+| 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
 

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>