From a98e9e8a1cb582186e9337d072cf3410d869e47f Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Sat, 18 Mar 2023 19:47:33 +0900 Subject: [PATCH] chore(ground_segmentation): add recheck ground cluster option param (#3106) Signed-off-by: badai-nguyen --- .../ground_segmentation/docs/scan-ground-filter.md | 1 + .../scan_ground_filter_nodelet.hpp | 1 + .../src/scan_ground_filter_nodelet.cpp | 11 +++++++++-- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 6d044d6b47c3a..0c07ce600f625 100644 --- 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,
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 index f6a8ba4c8a89a..11572772a0d0e 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 @@ -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 index fa9d3d6d83665..953a9feb4d21e 100644 --- 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";