Skip to content

Commit 1dc9726

Browse files
knzo25knzo25
committed
feat: cuda accelerated version of the pointcloud concatenation
Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
1 parent 660ae1a commit 1dc9726

16 files changed

+857
-3
lines changed

‎sensing/autoware_cuda_pointcloud_preprocessor/CMakeLists.txt

+13
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,10 @@ include_directories(
3737
list(APPEND CUDA_NVCC_FLAGS "--expt-relaxed-constexpr -diag-suppress 20012") # cSpell: ignore expt
3838

3939
cuda_add_library(cuda_pointcloud_preprocessor_lib SHARED
40+
src/cuda_concatenate_data/cuda_combine_cloud_handler.cpp
41+
src/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.cu
42+
src/cuda_concatenate_data/cuda_cloud_collector.cpp
43+
src/cuda_concatenate_data/cuda_collector_matching_strategy.cpp
4044
src/cuda_pointcloud_preprocessor/cuda_pointcloud_preprocessor.cu
4145
src/cuda_pointcloud_preprocessor/common_kernels.cu
4246
src/cuda_pointcloud_preprocessor/organize_kernels.cu
@@ -65,14 +69,23 @@ target_include_directories(cuda_pointcloud_preprocessor_lib SYSTEM PRIVATE
6569

6670
# Targets
6771
ament_auto_add_library(cuda_pointcloud_preprocessor SHARED
72+
src/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.cpp
6873
src/cuda_pointcloud_preprocessor/cuda_pointcloud_preprocessor_node.cpp
6974
)
7075

7176
target_link_libraries(cuda_pointcloud_preprocessor
7277
${CUDA_LIBRARIES}
78+
${diagnostic_msgs_LIBRARIES}
79+
${diagnostic_msgs_LIBRARIES}
7380
cuda_pointcloud_preprocessor_lib
7481
)
7582

83+
# ========== Concatenate and Sync data ==========
84+
rclcpp_components_register_node(cuda_pointcloud_preprocessor
85+
PLUGIN "autoware::cuda_pointcloud_preprocessor::CudaPointCloudConcatenateDataSynchronizerComponent"
86+
EXECUTABLE cuda_concatenate_and_time_sync_node)
87+
88+
# ========== Concatenate and Sync data ==========
7689
rclcpp_components_register_node(cuda_pointcloud_preprocessor
7790
PLUGIN "autoware::cuda_pointcloud_preprocessor::CudaPointcloudPreprocessorNode"
7891
EXECUTABLE cuda_pointcloud_preprocessor_node

‎sensing/autoware_cuda_pointcloud_preprocessor/README.md

+4-3
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,10 @@ To alleviate this issue, this package reimplements most of the pipeline presente
1010

1111
A detailed description of each filter's algorithm is available in the following links.
1212

13-
| Filter Name | Description | Detail |
14-
| ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------- |
15-
| 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) |
13+
| Filter Name | Description | Detail |
14+
| ----------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------- |
15+
| 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) |
16+
| cuda_concatenate_and_time_sync_node | Implements pointcloud concatenation an synchronization following `autoware_pointcloud_preprocessor`'s CPU implementation. | [link](docs/cuda-concatenate-data.md) |
1617

1718
## (Optional) Future extensions / Unimplemented parts
1819

‎sensing/autoware_cuda_pointcloud_preprocessor/config/cuda_concatenate_and_time_sync_node.param.yaml

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
/**:
2+
ros__parameters:
3+
debug_mode: false
4+
has_static_tf_only: false
5+
rosbag_length: 10.0
6+
maximum_queue_size: 5
7+
timeout_sec: 0.2
8+
is_motion_compensated: true
9+
publish_synchronized_pointcloud: true
10+
keep_input_frame_in_synchronized_pointcloud: true
11+
publish_previous_but_late_pointcloud: false
12+
synchronized_pointcloud_postfix: pointcloud
13+
input_twist_topic_type: twist
14+
input_topics: [
15+
"/sensing/lidar/right/pointcloud_before_sync",
16+
"/sensing/lidar/top/pointcloud_before_sync",
17+
"/sensing/lidar/left/pointcloud_before_sync",
18+
]
19+
output_frame: base_link
20+
matching_strategy:
21+
type: naive

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

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# cuda_concatenate_and_time_synchronize_node
2+
3+
This package is a cuda accelerated version of the one available in [autoware_cuda_pointcloud_preprocessor](../autoware_pointcloud_preprocessor).
4+
As this node is templated, the overall design, algorithm, inputs, and outputs are the same.
5+
6+
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

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#pragma once
16+
17+
#include "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp"
18+
#include "cuda_combine_cloud_handler.hpp"

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

+53
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#pragma once
16+
17+
#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp"
18+
#include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp"
19+
20+
namespace autoware::pointcloud_preprocessor
21+
{
22+
23+
template <>
24+
class CombineCloudHandler<CudaPointCloud2Traits> : public CombineCloudHandlerBase
25+
{
26+
protected:
27+
struct CudaConcatStruct
28+
{
29+
cudaStream_t stream;
30+
std::unique_ptr<CudaPointCloud2Traits::PointCloudMessage> cloud_ptr;
31+
std::size_t max_pointcloud_size{0};
32+
};
33+
34+
std::unordered_map<std::string, CudaConcatStruct> cuda_concat_struct_map_;
35+
std::unique_ptr<CudaPointCloud2Traits::PointCloudMessage> concatenated_cloud_ptr_;
36+
std::size_t max_concat_pointcloud_size_{0};
37+
std::mutex mutex_;
38+
39+
public:
40+
CombineCloudHandler(
41+
rclcpp::Node & node, const std::vector<std::string> & input_topics, std::string output_frame,
42+
bool is_motion_compensated, bool publish_synchronized_pointcloud,
43+
bool keep_input_frame_in_synchronized_pointcloud, bool has_static_tf_only);
44+
45+
ConcatenatedCloudResult<CudaPointCloud2Traits> combine_pointclouds(
46+
std::unordered_map<
47+
std::string, typename CudaPointCloud2Traits::PointCloudMessage::ConstSharedPtr> &
48+
topic_to_cloud_map);
49+
50+
void allocate_pointclouds() override;
51+
};
52+
53+
} // namespace autoware::pointcloud_preprocessor

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

+57
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__CUDA_CONCATENATE_DATA__CUDA_COMBINE_CLOUD_HANDLER_KERNEL_HPP_
16+
#define AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__CUDA_CONCATENATE_DATA__CUDA_COMBINE_CLOUD_HANDLER_KERNEL_HPP_
17+
18+
#include <cuda_runtime.h>
19+
20+
#include <cstdint>
21+
22+
namespace autoware::pointcloud_preprocessor
23+
{
24+
25+
struct TransformStruct
26+
{
27+
float translation_x;
28+
float translation_y;
29+
float translation_z;
30+
float m11;
31+
float m12;
32+
float m13;
33+
float m21;
34+
float m22;
35+
float m23;
36+
float m31;
37+
float m32;
38+
float m33;
39+
};
40+
41+
struct PointTypeStruct
42+
{
43+
float x;
44+
float y;
45+
float z;
46+
std::uint8_t intensity;
47+
std::uint8_t return_type;
48+
std::uint16_t channel;
49+
};
50+
51+
void transform_launch(
52+
const PointTypeStruct * input_points, int num_points, TransformStruct transform,
53+
PointTypeStruct * output_points, cudaStream_t & stream);
54+
55+
} // namespace autoware::pointcloud_preprocessor
56+
57+
#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

+42
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#pragma once
16+
17+
#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp"
18+
#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp"
19+
20+
#include <cuda_blackboard/cuda_blackboard_publisher.hpp>
21+
#include <cuda_blackboard/cuda_blackboard_subscriber.hpp>
22+
#include <cuda_blackboard/cuda_pointcloud2.hpp>
23+
#include <rclcpp/rclcpp.hpp>
24+
25+
namespace autoware::cuda_pointcloud_preprocessor
26+
{
27+
28+
class CudaPointCloudConcatenateDataSynchronizerComponent
29+
: public autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponentTemplated<
30+
autoware::pointcloud_preprocessor::CudaPointCloud2Traits>
31+
{
32+
public:
33+
explicit CudaPointCloudConcatenateDataSynchronizerComponent(
34+
const rclcpp::NodeOptions & node_options)
35+
: autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponentTemplated<
36+
autoware::pointcloud_preprocessor::CudaPointCloud2Traits>(node_options)
37+
{
38+
}
39+
~CudaPointCloudConcatenateDataSynchronizerComponent() override = default;
40+
};
41+
42+
} // namespace autoware::cuda_pointcloud_preprocessor

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

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#pragma once
16+
17+
#include <cuda_blackboard/cuda_blackboard_publisher.hpp>
18+
#include <cuda_blackboard/cuda_blackboard_subscriber.hpp>
19+
#include <cuda_blackboard/cuda_pointcloud2.hpp>
20+
21+
namespace autoware::pointcloud_preprocessor
22+
{
23+
24+
struct CudaPointCloud2Traits
25+
{
26+
using PointCloudMessage = cuda_blackboard::CudaPointCloud2;
27+
using PublisherType = cuda_blackboard::CudaBlackboardPublisher<PointCloudMessage>;
28+
using SubscriberType = cuda_blackboard::CudaBlackboardSubscriber<PointCloudMessage>;
29+
};
30+
31+
} // namespace autoware::pointcloud_preprocessor

‎sensing/autoware_cuda_pointcloud_preprocessor/launch/cuda_concatenate_and_time_sync_node.launch.xml

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
<launch>
2+
<arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
3+
<arg name="output" default="/sensing/lidar/concatenated/pointcloud"/>
4+
<!-- Parameter -->
5+
<arg name="param_file" default="$(find-pkg-share autoware_cuda_pointcloud_preprocessor)/config/cuda_concatenate_and_time_sync_node.param.yaml"/>
6+
<node pkg="autoware_cuda_pointcloud_preprocessor" exec="cuda_concatenate_and_time_sync_node" name="cuda_concatenate_and_time_sync_node" output="screen">
7+
<remap from="~/input/twist" to="$(var input/twist)"/>
8+
<remap from="output" to="$(var output)"/>
9+
<param from="$(var param_file)"/>
10+
</node>
11+
</launch>

0 commit comments

Comments
 (0)