feat(autoware_lidar_centerpoint): added a check to notify if we are dropping pillars (autowarefoundation#9488) (#1676)

* feat: added a check to notify if we are dropping pillars



* chore: updated text



* chore: throttled the message



---------

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>
Co-authored-by: Kenzo Lobos Tsunekawa <[email protected]>

Author: technolojin

perception/lidar_centerpoint/lib/centerpoint_trt.cpp

@@ -115,15 +115,28 @@ 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;
   }
 
   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_);
+  }
+
   return true;
 }