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 a1d12bcb56384..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 @@ -21,6 +21,7 @@ #include #include +#include namespace autoware::image_projection_based_fusion { @@ -35,6 +36,11 @@ 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, + bool & is_num_pillars_within_range); + protected: bool preprocess( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, 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 7ff80339930d8..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 @@ -38,6 +38,42 @@ 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, 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( + 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); + + // 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; +} + 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 4b531e34877d6..292e53ba020ed 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 @@ -18,12 +18,12 @@ #include "autoware/lidar_centerpoint/cuda_utils.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 +#include -#include "sensor_msgs/msg/point_cloud2.hpp" +#include +#include #include #include @@ -45,15 +45,17 @@ class CenterPointTRT virtual ~CenterPointTRT(); bool detect( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d, bool & is_num_pillars_within_range); + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d, + bool & is_num_pillars_within_range); protected: void initPtr(); void initTrt(const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param); 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 b2f8fde46bf63..673f4bbf71262 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -23,6 +23,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -44,12 +48,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_; std::vector class_names_; 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 18d4f3abb3703..50ccf5d9119ca 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 @@ -35,8 +35,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..f7eb6b884428f 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 @@ + @@ -30,6 +31,7 @@ + diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index d7fd2b0f30697..bee569605ffd3 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -170,8 +170,9 @@ void CenterPointTRT::initTrt( } bool CenterPointTRT::detect( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d, bool & is_num_pillars_within_range) + const std::shared_ptr & input_pointcloud_msg_ptr, + const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d, + bool & is_num_pillars_within_range) { is_num_pillars_within_range = true; @@ -180,7 +181,7 @@ bool CenterPointTRT::detect( 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(rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect."); return false; } @@ -208,9 +209,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/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 066bc5d995228..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 @@ -60,10 +63,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 +76,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 +87,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 208c96a369182..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 @@ -44,10 +44,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) @@ -55,12 +55,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( @@ -73,9 +75,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/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/package.xml b/perception/autoware_lidar_centerpoint/package.xml index 6cabac29d7e40..4284cdde1e0bf 100644 --- a/perception/autoware_lidar_centerpoint/package.xml +++ b/perception/autoware_lidar_centerpoint/package.xml @@ -18,6 +18,7 @@ autoware_perception_msgs autoware_tensorrt_common autoware_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 ae6bb320b6e70..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) @@ -110,9 +109,10 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti diagnostics_interface_ptr_ = std::make_unique(this, "centerpoint_trt"); - 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}); @@ -134,7 +134,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(); @@ -150,7 +150,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; }