chore(ground_segmentation): add recheck ground cluster option param (a…

…utowarefoundation#3106) Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
h-ohta · Mar 18, 2023 · a98e9e8 · a98e9e8
1 parent 104ebf7
commit a98e9e8
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 2 deletions.
diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md
@@ -47,6 +47,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 | `gnd_grid_buffer_size`            | uint16 | 4             | Number of grids using to estimate local ground slope,<br /> applied only for elevation_grid_mode                                                  |
 | `low_priority_region_x`           | float  | -20.0         | The non-zero x threshold in back side from which small objects detection is low priority [m]                                                      |
 | `elevation_grid_mode`             | bool   | true          | Elevation grid scan mode option                                                                                                                   |
+| `use_recheck_ground_cluster`      | bool   | true          | Enable recheck ground cluster                                                                                                                     |
 
 ## Assumptions / Known limits
 

diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
@@ -170,6 +170,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   double                                    // minimum height threshold regardless the slope,
     split_height_distance_;                 // useful for close points
   bool use_virtual_ground_point_;
+  bool use_recheck_ground_cluster_;  // to enable recheck ground cluster
   size_t radial_dividers_num_;
   VehicleInfo vehicle_info_;
 

diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
@@ -54,6 +54,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
     split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2);
     split_height_distance_ = declare_parameter("split_height_distance", 0.2);
     use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true);
+    use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true);
     radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
     vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
 
@@ -356,7 +357,9 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
       // move to new grid
       if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) {
         // check if the prev grid have ground point cloud
-        recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices);
+        if (use_recheck_ground_cluster_) {
+          recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices);
+        }
         curr_gnd_grid.radius = ground_cluster.getAverageRadius();
         curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();
         curr_gnd_grid.max_height = ground_cluster.getMaxHeight();
@@ -611,7 +614,11 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter(
       get_logger(),
       "Setting use_virtual_ground_point to: " << std::boolalpha << use_virtual_ground_point_);
   }
-
+  if (get_param(p, "use_recheck_ground_cluster", use_recheck_ground_cluster_)) {
+    RCLCPP_DEBUG_STREAM(
+      get_logger(),
+      "Setting use_recheck_ground_cluster to: " << std::boolalpha << use_recheck_ground_cluster_);
+  }
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
   result.reason = "success";