From 3443a7c856563759a68e778a3380ca7c2f321349 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 27 Nov 2024 17:17:14 +0900 Subject: [PATCH 1/3] feat: added a check to notify if we are dropping pillars Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lib/centerpoint_trt.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 5f81b70ab6d15..8c0c5e98d9671 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -144,6 +144,19 @@ bool CenterPointTRT::detect( 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_WARN( + rclcpp::get_logger("lidar_centerpoint"), + "The actual number of pillars exceeds (%u) its maximum value (%zu). " + "Please considering increasing it since it may limit the detection performance.", + num_pillars, config_.max_voxel_size_); + } + return true; } From 7d3691b5751fda06c9199c964a02f605b5ecb1f6 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 27 Nov 2024 17:44:28 +0900 Subject: [PATCH 2/3] chore: updated text Signed-off-by: Kenzo Lobos-Tsunekawa --- perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 8c0c5e98d9671..13123cf0ddc0a 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -152,7 +152,7 @@ bool CenterPointTRT::detect( if (num_pillars >= config_.max_voxel_size_) { RCLCPP_WARN( rclcpp::get_logger("lidar_centerpoint"), - "The actual number of pillars exceeds (%u) its maximum value (%zu). " + "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_); } From 82de8bde4a73dce64bbbcd2ec0ddca5fff222a82 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 27 Nov 2024 21:06:50 +0900 Subject: [PATCH 3/3] chore: throttled the message Signed-off-by: Kenzo Lobos-Tsunekawa --- .../autoware_lidar_centerpoint/lib/centerpoint_trt.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 13123cf0ddc0a..6ee2b3733bdb0 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -135,8 +135,7 @@ bool CenterPointTRT::detect( 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."); + RCLCPP_WARN(rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect."); return false; } @@ -150,8 +149,9 @@ bool CenterPointTRT::detect( cudaMemcpy(&num_pillars, num_voxels_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); if (num_pillars >= config_.max_voxel_size_) { - RCLCPP_WARN( - rclcpp::get_logger("lidar_centerpoint"), + 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_);