chore(ground_segmentation): add tuning param (autowarefoundation#6753)

* chore(ground_segmentation): add tuning param

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: and param into yaml file

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactor

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

---------

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Author: badai-nguyen

perception/ground_segmentation/config/ground_segmentation.param.yaml

@@ -33,3 +33,4 @@
         center_pcl_shift: 0.0
         radial_divider_angle_deg: 1.0
         use_recheck_ground_cluster: true
+        use_lowest_point: true

perception/ground_segmentation/config/scan_ground_filter.param.yaml

@@ -15,3 +15,4 @@
     center_pcl_shift: 0.0
     radial_divider_angle_deg: 1.0
     use_recheck_ground_cluster: true
+    use_lowest_point: true

perception/ground_segmentation/docs/scan-ground-filter.md

@@ -50,6 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 | `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                                                                                                                                                                                                                                                                                                                                    |
+| `use_lowest_point`                | bool   | true          | to select lowest point for reference in recheck ground cluster, otherwise select middle point                                                                                                                                                                                                                                                                    |
 
 ## Assumptions / Known limits
 

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -194,6 +194,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
     split_height_distance_;  // useful for close points
   bool use_virtual_ground_point_;
   bool use_recheck_ground_cluster_;  // to enable recheck ground cluster
+  bool use_lowest_point_;  // to select lowest point for reference in recheck ground cluster,
+                           // otherwise select middle point
   size_t radial_dividers_num_;
   VehicleInfo vehicle_info_;
 
@@ -258,7 +260,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
    * @param non_ground_indices Output non-ground PointCloud indices
    */
   void recheckGroundCluster(
-    PointsCentroid & gnd_cluster, const float non_ground_threshold,
+    PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
     pcl::PointIndices & non_ground_indices);
   /*!
    * Returns the resulting complementary PointCloud, one with the points kept

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -57,6 +57,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
     split_height_distance_ = declare_parameter<float>("split_height_distance");
     use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
     use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
+    use_lowest_point_ = declare_parameter<bool>("use_lowest_point");
     radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
     vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
 
@@ -316,14 +317,15 @@ void ScanGroundFilterComponent::checkBreakGndGrid(
 }
 
 void ScanGroundFilterComponent::recheckGroundCluster(
-  PointsCentroid & gnd_cluster, const float non_ground_threshold,
+  PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
   pcl::PointIndices & non_ground_indices)
 {
-  const float min_gnd_height = gnd_cluster.getMinHeight();
+  float reference_height =
+    use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight();
   const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef();
   const std::vector<float> & height_list = gnd_cluster.getHeightListRef();
   for (size_t i = 0; i < height_list.size(); ++i) {
-    if (height_list.at(i) >= min_gnd_height + non_ground_threshold) {
+    if (height_list.at(i) >= reference_height + non_ground_threshold) {
       non_ground_indices.indices.push_back(gnd_indices.indices.at(i));
     }
   }
@@ -403,7 +405,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
       if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) {
         // check if the prev grid have ground point cloud
         if (use_recheck_ground_cluster_) {
-          recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices);
+          recheckGroundCluster(
+            ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices);
         }
         curr_gnd_grid.radius = ground_cluster.getAverageRadius();
         curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();

perception/ground_segmentation/test/test_scan_ground_filter.cpp

@@ -83,6 +83,7 @@ class ScanGroundFilterTest : public ::testing::Test
       rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_));
     parameters.emplace_back(
       rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_));
+    parameters.emplace_back(rclcpp::Parameter("use_lowest_point", use_lowest_point_));
 
     options.parameter_overrides(parameters);
 
@@ -157,6 +158,7 @@ class ScanGroundFilterTest : public ::testing::Test
     center_pcl_shift_ = params["center_pcl_shift"].as<float>();
     radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as<float>();
     use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as<bool>();
+    use_lowest_point_ = params["use_lowest_point"].as<bool>();
   }
 
   float global_slope_max_angle_deg_ = 0.0;
@@ -174,6 +176,7 @@ class ScanGroundFilterTest : public ::testing::Test
   float center_pcl_shift_;
   float radial_divider_angle_deg_;
   bool use_recheck_ground_cluster_;
+  bool use_lowest_point_;
 };
 
 TEST_F(ScanGroundFilterTest, TestCase1)