Skip to content

Commit 63196f5

Browse files
knzo25amadeuszszbadai-nguyen
authored and
PanConChicharron
committed
feat(autoware_lidar_centerpoint): added the cuda_blackboard to centerpoint (autowarefoundation#9453)
* feat: introduced the cuda transport layer (cuda blackboard) to centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: fixed compilation issue on pointpainting Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * fix: fixed compile errors in the ml models Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * fix: fixed standalone non-composed launcher Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: ci/cd Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: clang tidy related fix Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: removed non applicable override (point painting does not support the blackboard yet) Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: temporarily ignoring warning until pointpainting also supports the blackboard Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: ignoring spell Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: removed the deprecated compatible subs option in the constructor Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: bump the cuda blackboard version in the build depends Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * Update perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp Co-authored-by: badai nguyen <[email protected]> * Update perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp Co-authored-by: badai nguyen <[email protected]> --------- Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> Co-authored-by: Amadeusz Szymko <[email protected]> Co-authored-by: badai nguyen <[email protected]>
1 parent c0346d5 commit 63196f5

File tree

16 files changed

+139
-87
lines changed

16 files changed

+139
-87
lines changed

‎build_depends_humble.repos‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ repositories:
4949
universe/external/cuda_blackboard:
5050
type: git
5151
url: https://github.com/autowarefoundation/cuda_blackboard.git
52-
version: v0.1.1
52+
version: v0.2.0
5353
universe/external/negotiated:
5454
type: git
5555
url: https://github.com/osrf/negotiated.git

‎perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,9 @@
2121

2222
#include <memory>
2323
#include <string>
24+
#include <vector>
25+
26+
#pragma clang diagnostic ignored "-Woverloaded-virtual" // cSpell:ignore Woverloaded
2427

2528
namespace autoware::image_projection_based_fusion
2629
{
@@ -35,10 +38,14 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT
3538
const autoware::lidar_centerpoint::DensificationParam & densification_param,
3639
const autoware::lidar_centerpoint::CenterPointConfig & config);
3740

41+
bool detect(
42+
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
43+
std::vector<autoware::lidar_centerpoint::Box3D> & det_boxes3d,
44+
bool & is_num_pillars_within_range);
45+
3846
protected:
3947
bool preprocess(
40-
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg,
41-
const tf2_ros::Buffer & tf_buffer) override;
48+
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer);
4249

4350
std::unique_ptr<image_projection_based_fusion::VoxelGenerator> vg_ptr_pp_{nullptr};
4451
};

‎perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,42 @@ PointPaintingTRT::PointPaintingTRT(
3838
std::make_unique<image_projection_based_fusion::VoxelGenerator>(densification_param, config_);
3939
}
4040

41+
bool PointPaintingTRT::detect(
42+
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
43+
std::vector<autoware::lidar_centerpoint::Box3D> & det_boxes3d, bool & is_num_pillars_within_range)
44+
{
45+
is_num_pillars_within_range = true;
46+
47+
CHECK_CUDA_ERROR(cudaMemsetAsync(
48+
encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_));
49+
CHECK_CUDA_ERROR(
50+
cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_));
51+
if (!preprocess(input_pointcloud_msg, tf_buffer)) {
52+
RCLCPP_WARN_STREAM(
53+
rclcpp::get_logger("pointpainting"), "Fail to preprocess and skip to detect.");
54+
return false;
55+
}
56+
inference();
57+
postProcess(det_boxes3d);
58+
59+
// Check the actual number of pillars after inference to avoid unnecessary synchronization.
60+
unsigned int num_pillars = 0;
61+
CHECK_CUDA_ERROR(
62+
cudaMemcpy(&num_pillars, num_voxels_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost));
63+
64+
if (num_pillars >= config_.max_voxel_size_) {
65+
rclcpp::Clock clock{RCL_ROS_TIME};
66+
RCLCPP_WARN_THROTTLE(
67+
rclcpp::get_logger("pointpainting"), clock, 1000,
68+
"The actual number of pillars (%u) exceeds its maximum value (%zu). "
69+
"Please considering increasing it since it may limit the detection performance.",
70+
num_pillars, config_.max_voxel_size_);
71+
is_num_pillars_within_range = false;
72+
}
73+
74+
return true;
75+
}
76+
4177
bool PointPaintingTRT::preprocess(
4278
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
4379
{

‎perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -18,12 +18,12 @@
1818
#include "autoware/lidar_centerpoint/cuda_utils.hpp"
1919
#include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp"
2020
#include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp"
21-
#include "pcl/point_cloud.h"
22-
#include "pcl/point_types.h"
2321

2422
#include <autoware/tensorrt_common/tensorrt_common.hpp>
23+
#include <cuda_blackboard/cuda_pointcloud2.hpp>
2524

26-
#include "sensor_msgs/msg/point_cloud2.hpp"
25+
#include <pcl/point_cloud.h>
26+
#include <pcl/point_types.h>
2727

2828
#include <memory>
2929
#include <string>
@@ -45,15 +45,17 @@ class CenterPointTRT
4545
virtual ~CenterPointTRT();
4646

4747
bool detect(
48-
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
49-
std::vector<Box3D> & det_boxes3d, bool & is_num_pillars_within_range);
48+
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
49+
const tf2_ros::Buffer & tf_buffer, std::vector<Box3D> & det_boxes3d,
50+
bool & is_num_pillars_within_range);
5051

5152
protected:
5253
void initPtr();
5354
void initTrt(const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param);
5455

5556
virtual bool preprocess(
56-
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer);
57+
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
58+
const tf2_ros::Buffer & tf_buffer);
5759

5860
void inference();
5961

‎perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,10 @@
2323
#include <autoware_utils/ros/diagnostics_interface.hpp>
2424
#include <autoware_utils/ros/published_time_publisher.hpp>
2525
#include <autoware_utils/system/stop_watch.hpp>
26+
#include <cuda_blackboard/cuda_adaptation.hpp>
27+
#include <cuda_blackboard/cuda_blackboard_subscriber.hpp>
28+
#include <cuda_blackboard/cuda_pointcloud2.hpp>
29+
#include <cuda_blackboard/negotiated_types.hpp>
2630
#include <rclcpp/rclcpp.hpp>
2731

2832
#include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
@@ -44,12 +48,14 @@ class LidarCenterPointNode : public rclcpp::Node
4448
explicit LidarCenterPointNode(const rclcpp::NodeOptions & node_options);
4549

4650
private:
47-
void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg);
51+
void pointCloudCallback(
52+
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg);
4853

4954
tf2_ros::Buffer tf_buffer_;
5055
tf2_ros::TransformListener tf_listener_{tf_buffer_};
5156

52-
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
57+
std::unique_ptr<cuda_blackboard::CudaBlackboardSubscriber<cuda_blackboard::CudaPointCloud2>>
58+
pointcloud_sub_;
5359
rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_;
5460

5561
std::vector<std::string> class_names_;

‎perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -15,19 +15,21 @@
1515
#ifndef AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_
1616
#define AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_
1717

18-
#include "tf2_ros/buffer.h"
19-
#include "tf2_ros/transform_listener.h"
18+
#include <cuda_blackboard/cuda_pointcloud2.hpp>
19+
20+
#include <tf2_ros/buffer.h>
21+
#include <tf2_ros/transform_listener.h>
2022

2123
#include <list>
2224
#include <string>
2325
#include <utility>
2426
#ifdef ROS_DISTRO_GALACTIC
25-
#include "tf2_sensor_msgs/tf2_sensor_msgs.h"
27+
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
2628
#else
27-
#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
29+
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
2830
#endif
2931

30-
#include "autoware/lidar_centerpoint/cuda_utils.hpp"
32+
#include <memory>
3133

3234
namespace autoware::lidar_centerpoint
3335
{
@@ -50,10 +52,7 @@ class DensificationParam
5052

5153
struct PointCloudWithTransform
5254
{
53-
cuda::unique_ptr<float[]> points_d{nullptr};
54-
std_msgs::msg::Header header;
55-
std::size_t num_points{0};
56-
std::size_t point_step{0};
55+
std::shared_ptr<const cuda_blackboard::CudaPointCloud2> input_pointcloud_msg_ptr;
5756
Eigen::Affine3f affine_past2world;
5857
};
5958

@@ -63,8 +62,8 @@ class PointCloudDensification
6362
explicit PointCloudDensification(const DensificationParam & param);
6463

6564
bool enqueuePointCloud(
66-
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
67-
cudaStream_t stream);
65+
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
66+
const tf2_ros::Buffer & tf_buffer);
6867

6968
double getCurrentTimestamp() const { return current_timestamp_; }
7069
Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; }
@@ -80,7 +79,8 @@ class PointCloudDensification
8079

8180
private:
8281
void enqueue(
83-
const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine, cudaStream_t stream);
82+
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
83+
const Eigen::Affine3f & affine);
8484
void dequeue();
8585

8686
DensificationParam param_;

‎perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
#include "autoware/lidar_centerpoint/centerpoint_config.hpp"
1919
#include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp"
2020

21-
#include "sensor_msgs/msg/point_cloud2.hpp"
21+
#include <cuda_blackboard/cuda_pointcloud2.hpp>
2222

2323
#include <memory>
2424
#include <vector>
@@ -35,8 +35,8 @@ class VoxelGeneratorTemplate
3535
virtual std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) = 0;
3636

3737
bool enqueuePointCloud(
38-
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
39-
cudaStream_t stream);
38+
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
39+
const tf2_ros::Buffer & tf_buffer);
4040

4141
protected:
4242
std::unique_ptr<PointCloudDensification> pd_ptr_{nullptr};

‎perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
<load_composable_node target="$(var pointcloud_container_name)">
1818
<composable_node pkg="autoware_lidar_centerpoint" plugin="autoware::lidar_centerpoint::LidarCenterPointNode" name="lidar_centerpoint">
1919
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
20+
<remap from="~/input/pointcloud/cuda" to="$(var input/pointcloud)/cuda"/>
2021
<remap from="~/output/objects" to="$(var output/objects)"/>
2122
<param from="$(var model_param_path)" allow_substs="true"/>
2223
<param from="$(var ml_package_param_path)" allow_substs="true"/>
@@ -30,6 +31,7 @@
3031
<group unless="$(var use_pointcloud_container)">
3132
<node pkg="autoware_lidar_centerpoint" exec="autoware_lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
3233
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
34+
<remap from="~/input/pointcloud/cuda" to="$(var input/pointcloud)/cuda"/>
3335
<remap from="~/output/objects" to="$(var output/objects)"/>
3436
<param from="$(var model_param_path)" allow_substs="true"/>
3537
<param from="$(var ml_package_param_path)" allow_substs="true"/>

‎perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -170,8 +170,9 @@ void CenterPointTRT::initTrt(
170170
}
171171

172172
bool CenterPointTRT::detect(
173-
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
174-
std::vector<Box3D> & det_boxes3d, bool & is_num_pillars_within_range)
173+
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
174+
const tf2_ros::Buffer & tf_buffer, std::vector<Box3D> & det_boxes3d,
175+
bool & is_num_pillars_within_range)
175176
{
176177
is_num_pillars_within_range = true;
177178

@@ -180,7 +181,7 @@ bool CenterPointTRT::detect(
180181
CHECK_CUDA_ERROR(
181182
cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_));
182183

183-
if (!preprocess(input_pointcloud_msg, tf_buffer)) {
184+
if (!preprocess(input_pointcloud_msg_ptr, tf_buffer)) {
184185
RCLCPP_WARN(rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect.");
185186
return false;
186187
}
@@ -208,9 +209,10 @@ bool CenterPointTRT::detect(
208209
}
209210

210211
bool CenterPointTRT::preprocess(
211-
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
212+
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
213+
const tf2_ros::Buffer & tf_buffer)
212214
{
213-
bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream_);
215+
bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg_ptr, tf_buffer);
214216
if (!is_success) {
215217
return false;
216218
}

‎perception/autoware_lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,8 @@ All Rights Reserved 2019-2020.
2424
#include "autoware/lidar_centerpoint/cuda_utils.hpp"
2525
#include "autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp"
2626
#include "autoware/lidar_centerpoint/utils.hpp"
27-
#include "thrust/host_vector.h"
27+
28+
#include <thrust/host_vector.h>
2829

2930
namespace
3031
{

0 commit comments

Comments
 (0)