From 4686ce6aa0dcd7e89a0854c04eaa5c12676972ab Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 25 Nov 2024 11:46:57 +0900 Subject: [PATCH 1/6] feat: introduced the cuda transport layer (cuda blackboard) to centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa --- .../pointpainting_trt.hpp | 8 +++-- .../pointpainting_trt.cpp | 18 +++++++++++ .../lidar_centerpoint/centerpoint_trt.hpp | 14 +++++---- .../autoware/lidar_centerpoint/node.hpp | 10 ++++-- .../preprocess/pointcloud_densification.hpp | 24 +++++++------- .../preprocess/voxel_generator.hpp | 6 ++-- .../launch/lidar_centerpoint.launch.xml | 1 + .../lib/centerpoint_trt.cpp | 11 ++++--- .../preprocess/pointcloud_densification.cpp | 31 ++++++------------- .../lib/preprocess/voxel_generator.cpp | 20 ++++++------ .../autoware_lidar_centerpoint/package.xml | 1 + .../autoware_lidar_centerpoint/src/node.cpp | 11 ++++--- 12 files changed, 90 insertions(+), 65 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index a89ee3d02cd8b..c0fdfc632dbd2 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -20,6 +20,7 @@ #include #include +#include namespace autoware::image_projection_based_fusion { @@ -34,10 +35,13 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT const autoware::lidar_centerpoint::DensificationParam & densification_param, const autoware::lidar_centerpoint::CenterPointConfig & config); + bool detect( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + std::vector & det_boxes3d); + protected: bool preprocess( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const tf2_ros::Buffer & tf_buffer) override; + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); std::unique_ptr vg_ptr_pp_{nullptr}; }; diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index cc3eb17cc32ab..2189e310ca52c 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -38,6 +38,24 @@ PointPaintingTRT::PointPaintingTRT( std::make_unique(densification_param, config_); } +bool PointPaintingTRT::detect( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + std::vector & det_boxes3d) +{ + CHECK_CUDA_ERROR(cudaMemsetAsync( + encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_)); + if (!preprocess(input_pointcloud_msg, tf_buffer)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect."); + return false; + } + inference(); + postProcess(det_boxes3d); + return true; +} + bool PointPaintingTRT::preprocess( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer) { diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp index ce5ec3ea0abfe..bf97f02c61d59 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp @@ -19,10 +19,11 @@ #include "autoware/lidar_centerpoint/network/network_trt.hpp" #include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp" #include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "sensor_msgs/msg/point_cloud2.hpp" +#include + +#include +#include #include #include @@ -61,14 +62,15 @@ class CenterPointTRT virtual ~CenterPointTRT(); bool detect( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d); + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d); protected: void initPtr(); virtual bool preprocess( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer); void inference(); diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp index eb056ed14c57f..e52db6e722454 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -22,6 +22,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -43,12 +47,14 @@ class LidarCenterPointNode : public rclcpp::Node explicit LidarCenterPointNode(const rclcpp::NodeOptions & node_options); private: - void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg); + void pointCloudCallback( + const std::shared_ptr & input_pointcloud_msg); tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_{tf_buffer_}; - rclcpp::Subscription::SharedPtr pointcloud_sub_; + std::unique_ptr> + pointcloud_sub_; rclcpp::Publisher::SharedPtr objects_pub_; float score_threshold_{0.0}; diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp index 8934aed6dba82..c335b1ca8e06c 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp @@ -15,19 +15,21 @@ #ifndef AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ #define AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" +#include + +#include +#include #include #include #include #ifdef ROS_DISTRO_GALACTIC -#include "tf2_sensor_msgs/tf2_sensor_msgs.h" +#include #else -#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" +#include #endif -#include "autoware/lidar_centerpoint/cuda_utils.hpp" +#include namespace autoware::lidar_centerpoint { @@ -50,10 +52,7 @@ class DensificationParam struct PointCloudWithTransform { - cuda::unique_ptr points_d{nullptr}; - std_msgs::msg::Header header; - std::size_t num_points{0}; - std::size_t point_step{0}; + std::shared_ptr input_pointcloud_msg_ptr; Eigen::Affine3f affine_past2world; }; @@ -63,8 +62,8 @@ class PointCloudDensification explicit PointCloudDensification(const DensificationParam & param); bool enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - cudaStream_t stream); + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer); double getCurrentTimestamp() const { return current_timestamp_; } Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } @@ -80,7 +79,8 @@ class PointCloudDensification private: void enqueue( - const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine, cudaStream_t stream); + const std::shared_ptr & input_pointcloud_msg_ptr, + const Eigen::Affine3f & affine); void dequeue(); DensificationParam param_; diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp index 66384d192ae43..4ecbb3aafda1f 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp @@ -18,7 +18,7 @@ #include "autoware/lidar_centerpoint/centerpoint_config.hpp" #include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" +#include #include #include @@ -34,8 +34,8 @@ class VoxelGeneratorTemplate virtual std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) = 0; bool enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - cudaStream_t stream); + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer); protected: std::unique_ptr pd_ptr_{nullptr}; diff --git a/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 639502d52806a..9a77a707bab8f 100644 --- a/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -17,6 +17,7 @@ + diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 5f81b70ab6d15..b6a5dd24bd08b 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -126,15 +126,15 @@ void CenterPointTRT::initPtr() } bool CenterPointTRT::detect( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d) + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d) { CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); CHECK_CUDA_ERROR( cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_)); - if (!preprocess(input_pointcloud_msg, tf_buffer)) { + if (!preprocess(input_pointcloud_msg_ptr, tf_buffer)) { RCLCPP_WARN_STREAM( rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect."); return false; @@ -148,9 +148,10 @@ bool CenterPointTRT::detect( } bool CenterPointTRT::preprocess( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer) + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer) { - bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream_); + bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg_ptr, tf_buffer); if (!is_success) { return false; } diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp b/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp index 066bc5d995228..c72c390b434a4 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp @@ -60,10 +60,10 @@ PointCloudDensification::PointCloudDensification(const DensificationParam & para } bool PointCloudDensification::enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - cudaStream_t stream) + const std::shared_ptr & pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer) { - const auto header = pointcloud_msg.header; + const auto header = pointcloud_msg_ptr->header; if (param_.pointcloud_cache_size() > 1) { auto transform_world2current = @@ -73,9 +73,9 @@ bool PointCloudDensification::enqueuePointCloud( } auto affine_world2current = transformToEigen(transform_world2current.get()); - enqueue(pointcloud_msg, affine_world2current, stream); + enqueue(pointcloud_msg_ptr, affine_world2current); } else { - enqueue(pointcloud_msg, Eigen::Affine3f::Identity(), stream); + enqueue(pointcloud_msg_ptr, Eigen::Affine3f::Identity()); } dequeue(); @@ -84,24 +84,13 @@ bool PointCloudDensification::enqueuePointCloud( } void PointCloudDensification::enqueue( - const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current, - cudaStream_t stream) + const std::shared_ptr & msg_ptr, + const Eigen::Affine3f & affine_world2current) { affine_world2current_ = affine_world2current; - current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); - - assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 0); - auto points_d = cuda::make_unique( - sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float)); - CHECK_CUDA_ERROR(cudaMemcpyAsync( - points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, - cudaMemcpyHostToDevice, stream)); - - PointCloudWithTransform pointcloud = { - std::move(points_d), msg.header, msg.width * msg.height, msg.point_step, - affine_world2current.inverse()}; - - pointcloud_cache_.push_front(std::move(pointcloud)); + current_timestamp_ = rclcpp::Time(msg_ptr->header.stamp).seconds(); + PointCloudWithTransform pointcloud = {msg_ptr, affine_world2current.inverse()}; + pointcloud_cache_.push_front(pointcloud); } void PointCloudDensification::dequeue() diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp index f975449530e5c..aa285254921d5 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -43,10 +43,10 @@ VoxelGeneratorTemplate::VoxelGeneratorTemplate( } bool VoxelGeneratorTemplate::enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - cudaStream_t stream) + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer) { - return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream); + return pd_ptr_->enqueuePointCloud(input_pointcloud_msg_ptr, tf_buffer); } std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t stream) @@ -54,12 +54,14 @@ std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t s std::size_t point_counter = 0; for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter); pc_cache_iter++) { - auto sweep_num_points = pc_cache_iter->num_points; - auto point_step = pc_cache_iter->point_step; + const auto & input_pointcloud_msg_ptr = pc_cache_iter->input_pointcloud_msg_ptr; + auto sweep_num_points = input_pointcloud_msg_ptr->height * input_pointcloud_msg_ptr->width; + auto point_step = input_pointcloud_msg_ptr->point_step; auto affine_past2current = pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; float time_lag = static_cast( - pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds()); + pd_ptr_->getCurrentTimestamp() - + rclcpp::Time(input_pointcloud_msg_ptr->header.stamp).seconds()); if (point_counter + sweep_num_points > config_.cloud_capacity_) { RCLCPP_WARN_STREAM( @@ -72,9 +74,9 @@ std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t s static_assert(std::is_same::value); static_assert(!Eigen::Matrix4f::IsRowMajor, "matrices should be col-major."); generateSweepPoints_launch( - pc_cache_iter->points_d.get(), sweep_num_points, point_step / sizeof(float), time_lag, - affine_past2current.matrix().data(), config_.point_feature_size_, - points_d + config_.point_feature_size_ * point_counter, stream); + reinterpret_cast(input_pointcloud_msg_ptr->data.get()), sweep_num_points, + point_step / sizeof(float), time_lag, affine_past2current.matrix().data(), + config_.point_feature_size_, points_d + config_.point_feature_size_ * point_counter, stream); point_counter += sweep_num_points; } diff --git a/perception/autoware_lidar_centerpoint/package.xml b/perception/autoware_lidar_centerpoint/package.xml index 4f33fd14dbb8f..b27b91601e4fa 100644 --- a/perception/autoware_lidar_centerpoint/package.xml +++ b/perception/autoware_lidar_centerpoint/package.xml @@ -16,6 +16,7 @@ autoware_perception_msgs autoware_tensorrt_common autoware_universe_utils + cuda_blackboard pcl_ros rclcpp rclcpp_components diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index 5b92e5c811a23..5166b9678ef81 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -111,9 +111,10 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); - pointcloud_sub_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), - std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1)); + pointcloud_sub_ = + std::make_unique>( + *this, "~/input/pointcloud", false, + std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1)); objects_pub_ = this->create_publisher( "~/output/objects", rclcpp::QoS{1}); @@ -136,7 +137,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti } void LidarCenterPointNode::pointCloudCallback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg) + const std::shared_ptr & input_pointcloud_msg) { const auto objects_sub_count = objects_pub_->get_subscription_count() + objects_pub_->get_intra_process_subscription_count(); @@ -149,7 +150,7 @@ void LidarCenterPointNode::pointCloudCallback( } std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_success = detector_ptr_->detect(input_pointcloud_msg, tf_buffer_, det_boxes3d); if (!is_success) { return; } From 46d5dcdfe8137a125b84671d0a9984efe6f4dc64 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 25 Nov 2024 12:52:26 +0900 Subject: [PATCH 2/6] chore: fixed compilation issue on pointpainting Signed-off-by: Kenzo Lobos-Tsunekawa --- .../pointpainting_fusion/pointpainting_trt.hpp | 2 +- .../src/pointpainting_fusion/pointpainting_trt.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index c0fdfc632dbd2..ec45b3e31f3d3 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -37,7 +37,7 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d); + std::vector & det_boxes3d); protected: bool preprocess( diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index 2189e310ca52c..a2c3d51c70101 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -40,7 +40,7 @@ PointPaintingTRT::PointPaintingTRT( bool PointPaintingTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d) + std::vector & det_boxes3d) { CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); From 3893f193332392fd0ee9384e427f6d34e3aa2b5f Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 17 Mar 2025 15:05:41 +0900 Subject: [PATCH 3/6] fix: fixed compile errors in the ml models Signed-off-by: Kenzo Lobos-Tsunekawa --- .../pointpainting_trt.hpp | 3 ++- .../pointpainting_trt.cpp | 20 ++++++++++++++++++- .../autoware_lidar_centerpoint/src/node.cpp | 2 +- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index 3da18950e2809..09a68225d4d68 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -38,7 +38,8 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d); + std::vector & det_boxes3d, + bool & is_num_pillars_within_range); protected: bool preprocess( diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index 084bba72d5016..d14e631ae5718 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -40,8 +40,10 @@ PointPaintingTRT::PointPaintingTRT( bool PointPaintingTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d) + std::vector & det_boxes3d, bool & is_num_pillars_within_range) { + is_num_pillars_within_range = true; + CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); CHECK_CUDA_ERROR( @@ -53,6 +55,22 @@ bool PointPaintingTRT::detect( } inference(); postProcess(det_boxes3d); + + // Check the actual number of pillars after inference to avoid unnecessary synchronization. + unsigned int num_pillars = 0; + CHECK_CUDA_ERROR( + cudaMemcpy(&num_pillars, num_voxels_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + if (num_pillars >= config_.max_voxel_size_) { + rclcpp::Clock clock{RCL_ROS_TIME}; + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("lidar_centerpoint"), clock, 1000, + "The actual number of pillars (%u) exceeds its maximum value (%zu). " + "Please considering increasing it since it may limit the detection performance.", + num_pillars, config_.max_voxel_size_); + is_num_pillars_within_range = false; + } + return true; } diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index b1e05a6e5ec31..6cf1602ca3286 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -151,7 +151,7 @@ void LidarCenterPointNode::pointCloudCallback( std::vector det_boxes3d; bool is_num_pillars_within_range = true; bool is_success = detector_ptr_->detect( - *input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); + input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } From 2bd110c6923512bf1d7c1cc40dc65690344f4b4e Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 19 Mar 2025 15:28:54 +0900 Subject: [PATCH 4/6] fix: fixed standalone non-composed launcher Signed-off-by: Kenzo Lobos-Tsunekawa --- .../launch/lidar_centerpoint.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 9a77a707bab8f..f7eb6b884428f 100644 --- a/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -31,6 +31,7 @@ + From 3cd4c5e5353193edff08f8ee7cf8f0cf9874dba6 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 19 Mar 2025 15:39:08 +0900 Subject: [PATCH 5/6] chore: ci/cd Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lib/postprocess/circle_nms_kernel.cu | 3 ++- .../lib/postprocess/postprocess_kernel.cu | 7 ++++--- .../lib/preprocess/pointcloud_densification.cpp | 13 ++++++++----- .../lib/preprocess/voxel_generator.cpp | 2 +- .../autoware_lidar_centerpoint/lib/ros_utils.cpp | 6 +++--- .../autoware_lidar_centerpoint/src/node.cpp | 15 +++++++-------- 6 files changed, 25 insertions(+), 21 deletions(-) diff --git a/perception/autoware_lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu b/perception/autoware_lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu index 63a7d1625f66a..1a97f99728a2f 100644 --- a/perception/autoware_lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu @@ -24,7 +24,8 @@ All Rights Reserved 2019-2020. #include "autoware/lidar_centerpoint/cuda_utils.hpp" #include "autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp" #include "autoware/lidar_centerpoint/utils.hpp" -#include "thrust/host_vector.h" + +#include namespace { diff --git a/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 81157e07991d1..5c7d9c0025654 100644 --- a/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -14,9 +14,10 @@ #include "autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp" #include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp" -#include "thrust/count.h" -#include "thrust/device_vector.h" -#include "thrust/sort.h" + +#include +#include +#include namespace { diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp b/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp index c72c390b434a4..40da1336d7883 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp @@ -14,17 +14,20 @@ #include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp" -#include "pcl_conversions/pcl_conversions.h" -#include "pcl_ros/transforms.hpp" +#include -#include "boost/optional.hpp" +#include +#include + +#include #include #include + #ifdef ROS_DISTRO_GALACTIC -#include "tf2_eigen/tf2_eigen.h" +#include #else -#include "tf2_eigen/tf2_eigen.hpp" +#include #endif namespace diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp index 13cc829ffa035..64d2836007420 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -17,7 +17,7 @@ #include "autoware/lidar_centerpoint/centerpoint_trt.hpp" #include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" -#include "sensor_msgs/point_cloud2_iterator.hpp" +#include #include #include diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index 4bb5e15934c75..32f0b2c00418e 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_centerpoint/ros_utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/math/constants.hpp" +#include +#include +#include #include #include diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index 6cf1602ca3286..4c9bee5dd5cec 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -14,26 +14,25 @@ #include "autoware/lidar_centerpoint/node.hpp" -#include "pcl_ros/transforms.hpp" +#include "autoware/lidar_centerpoint/centerpoint_config.hpp" +#include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_centerpoint/ros_utils.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" #include #include +#include #include #include #include #ifdef ROS_DISTRO_GALACTIC -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include #else -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include #endif -#include "autoware/lidar_centerpoint/centerpoint_config.hpp" -#include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp" -#include "autoware/lidar_centerpoint/ros_utils.hpp" -#include "autoware/lidar_centerpoint/utils.hpp" - namespace autoware::lidar_centerpoint { LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_options) From ffbfb88f2bb73cae87354b64466e99efe6477aee Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 19 Mar 2025 17:07:17 +0900 Subject: [PATCH 6/6] chore: clang tidy related fix Signed-off-by: Kenzo Lobos-Tsunekawa --- .../pointpainting_fusion/pointpainting_trt.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index 09a68225d4d68..65ae74c2601d3 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -43,7 +43,8 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT protected: bool preprocess( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + const tf2_ros::Buffer & tf_buffer) override; std::unique_ptr vg_ptr_pp_{nullptr}; };