diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 756882391183e..594ef1fe974b2 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/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 diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml index bc02a1ab7e6e7..35d494a620b19 100644 --- a/perception/ground_segmentation/config/scan_ground_filter.param.yaml +++ b/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 diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 21df6fa6ea1b9..d4eb9c6f3addf 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/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 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 index d017d06929702..d97bbaa2118e5 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/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 diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 7a0333d95075b..34573b564fa36 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -57,6 +57,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & split_height_distance_ = declare_parameter("split_height_distance"); use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); + use_lowest_point_ = declare_parameter("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 & 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(); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index 444a38ea44754..e48bd36d8c54e 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/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(); radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); + use_lowest_point_ = params["use_lowest_point"].as(); } 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)