feat(autoware_lidar_centerpoint): added a check to notify if we are d…

…ropping 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]>
tier4 · Dec 6, 2024 · adfecc2 · adfecc2
1 parent db51b36
commit adfecc2
Showing 1 changed file with 15 additions and 2 deletions.
diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/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;
 }