From 86e547cef77bc073112fb8335a04468ad164de04 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 4 Jul 2022 23:55:35 +0900 Subject: [PATCH 1/9] fix(ground_filter): expand range of cropped_box_filter Signed-off-by: badai-nguyen --- .../ground_segmentation/scan_ground_filter_nodelet.hpp | 1 + .../src/scan_ground_filter_nodelet.cpp | 10 +++++++++- 2 files changed, 10 insertions(+), 1 deletion(-) 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 191e99494dab..32afebbf34c1 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 @@ -113,6 +113,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter std::string base_frame_; std::string sensor_frame_; + float non_ground_heigh_thresh_ = 2.5f; // maximum height of non_ground object from ground double global_slope_max_angle_rad_; // radians double local_slope_max_angle_rad_; // radians double radial_divider_angle_rad_; // distance in rads between dividers diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index fa803c4afab0..4c50f36f132a 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -38,6 +38,8 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & // set initial parameters { base_frame_ = declare_parameter("base_frame", "base_link"); + non_ground_heigh_thresh_ = + static_cast(declare_parameter("non_ground_heigh_thresh", 2.5)); global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 6.0)); radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); @@ -168,7 +170,13 @@ void ScanGroundFilterComponent::classifyPointCloud( float global_slope = std::atan2(p->orig_point->z, p->radius); // check points which is far enough from previous point - if (global_slope > global_slope_max_angle) { + if ( + (height_from_gnd < -split_height_distance_) || + ((p->orig_point->z - ground_cluster.getAverageHeight()) < -split_height_distance_) || + (height_from_gnd > non_ground_heigh_thresh_)) { + // exclude pcl under ground or higher than limit + calculate_slope = false; + } else if (global_slope > global_slope_max_angle) { p->point_state = PointLabel::NON_GROUND; calculate_slope = false; } else if ( From 5a6779238ac49e9a85a145df7ab2b07bd3bb9d9f Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 7 Jul 2022 08:54:04 +0900 Subject: [PATCH 2/9] chore: typo Signed-off-by: badai-nguyen --- .../scan_ground_filter_nodelet.hpp | 3 ++- .../src/scan_ground_filter_nodelet.cpp | 12 +++++++----- 2 files changed, 9 insertions(+), 6 deletions(-) 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 32afebbf34c1..7ebb3236c321 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 @@ -113,7 +113,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter std::string base_frame_; std::string sensor_frame_; - float non_ground_heigh_thresh_ = 2.5f; // maximum height of non_ground object from ground + float non_ground_height_max_ = 2.5f; // maximum height of non_ground object from ground + float non_ground_height_min_ = 0.0f; // mimimum height of non_ground object from ground double global_slope_max_angle_rad_; // radians double local_slope_max_angle_rad_; // radians double radial_divider_angle_rad_; // distance in rads between dividers diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 4c50f36f132a..9a2e95f41b0d 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -38,8 +38,10 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & // set initial parameters { base_frame_ = declare_parameter("base_frame", "base_link"); - non_ground_heigh_thresh_ = - static_cast(declare_parameter("non_ground_heigh_thresh", 2.5)); + non_ground_height_max_ = + static_cast(declare_parameter("non_ground_height_max", 2.5)); + non_ground_height_min_ = + static_cast(declare_parameter("non_ground_height_min", 0.0)); global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 6.0)); radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); @@ -171,9 +173,9 @@ void ScanGroundFilterComponent::classifyPointCloud( float global_slope = std::atan2(p->orig_point->z, p->radius); // check points which is far enough from previous point if ( - (height_from_gnd < -split_height_distance_) || - ((p->orig_point->z - ground_cluster.getAverageHeight()) < -split_height_distance_) || - (height_from_gnd > non_ground_heigh_thresh_)) { + (height_from_gnd < non_ground_height_min_) || + ((p->orig_point->z - ground_cluster.getAverageHeight()) < non_ground_height_min_) || + (height_from_gnd > non_ground_height_max_)) { // exclude pcl under ground or higher than limit calculate_slope = false; } else if (global_slope > global_slope_max_angle) { From d79d3fe08485e11e04b1cf9e9ad1c3ea0eef6cde Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 6 Jul 2022 23:58:52 +0000 Subject: [PATCH 3/9] ci(pre-commit): autofix --- .../ground_segmentation/src/scan_ground_filter_nodelet.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 9a2e95f41b0d..ab664588e4c4 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -38,10 +38,8 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & // set initial parameters { base_frame_ = declare_parameter("base_frame", "base_link"); - non_ground_height_max_ = - static_cast(declare_parameter("non_ground_height_max", 2.5)); - non_ground_height_min_ = - static_cast(declare_parameter("non_ground_height_min", 0.0)); + non_ground_height_max_ = static_cast(declare_parameter("non_ground_height_max", 2.5)); + non_ground_height_min_ = static_cast(declare_parameter("non_ground_height_min", 0.0)); global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 6.0)); radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); From 23d85f527736364797018c706055ac3df4ab851b Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 7 Jul 2022 09:26:12 +0900 Subject: [PATCH 4/9] chore: add out_range point label Signed-off-by: badai-nguyen --- .../ground_segmentation/scan_ground_filter_nodelet.hpp | 1 + .../ground_segmentation/src/scan_ground_filter_nodelet.cpp | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) 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 7ebb3236c321..5f5eb23f76f7 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 @@ -54,6 +54,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter POINT_FOLLOW, UNKNOWN, VIRTUAL_GROUND, + OUT_OF_RANGE }; struct PointRef { diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index ab664588e4c4..2207289e1b26 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -39,7 +39,9 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & { base_frame_ = declare_parameter("base_frame", "base_link"); non_ground_height_max_ = static_cast(declare_parameter("non_ground_height_max", 2.5)); - non_ground_height_min_ = static_cast(declare_parameter("non_ground_height_min", 0.0)); + non_ground_height_min_ = static_cast(declare_parameter("non_ground_height_min", -0.2)); + // TODO(badai-nguyen) : adjust margin for non_ground_height_min_ based on the distance + // from LiDAR to allow a down-slope global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 6.0)); radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); @@ -175,6 +177,7 @@ void ScanGroundFilterComponent::classifyPointCloud( ((p->orig_point->z - ground_cluster.getAverageHeight()) < non_ground_height_min_) || (height_from_gnd > non_ground_height_max_)) { // exclude pcl under ground or higher than limit + p->point_state = PointLabel::OUT_OF_RANGE; calculate_slope = false; } else if (global_slope > global_slope_max_angle) { p->point_state = PointLabel::NON_GROUND; From c786f6b5c62fe488430222b64394cd12fe6492b5 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 7 Jul 2022 09:38:27 +0900 Subject: [PATCH 5/9] chore: typo Signed-off-by: badai-nguyen --- .../scan_ground_filter_nodelet.hpp | 4 ++-- .../src/scan_ground_filter_nodelet.cpp | 14 ++++++++------ 2 files changed, 10 insertions(+), 8 deletions(-) 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 5f5eb23f76f7..e612fbe44b46 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 @@ -114,8 +114,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter std::string base_frame_; std::string sensor_frame_; - float non_ground_height_max_ = 2.5f; // maximum height of non_ground object from ground - float non_ground_height_min_ = 0.0f; // mimimum height of non_ground object from ground + float maximum_detection_range_ = 2.5f; // maximum height of non_ground object from ground + float minimum_detection_range_ = 0.0f; // mimimum height of non_ground object from ground double global_slope_max_angle_rad_; // radians double local_slope_max_angle_rad_; // radians double radial_divider_angle_rad_; // distance in rads between dividers diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 2207289e1b26..a08cd9566671 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -38,9 +38,11 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & // set initial parameters { base_frame_ = declare_parameter("base_frame", "base_link"); - non_ground_height_max_ = static_cast(declare_parameter("non_ground_height_max", 2.5)); - non_ground_height_min_ = static_cast(declare_parameter("non_ground_height_min", -0.2)); - // TODO(badai-nguyen) : adjust margin for non_ground_height_min_ based on the distance + maximum_detection_range_ = + static_cast(declare_parameter("maximum_detection_range", 2.5)); + minimum_detection_range_ = + static_cast(declare_parameter("minimum_detection_range", -0.2)); + // TODO(badai-nguyen) : adjust margin for minimum_detection_range_ based on the distance // from LiDAR to allow a down-slope global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 6.0)); @@ -173,9 +175,9 @@ void ScanGroundFilterComponent::classifyPointCloud( float global_slope = std::atan2(p->orig_point->z, p->radius); // check points which is far enough from previous point if ( - (height_from_gnd < non_ground_height_min_) || - ((p->orig_point->z - ground_cluster.getAverageHeight()) < non_ground_height_min_) || - (height_from_gnd > non_ground_height_max_)) { + (height_from_gnd < minimum_detection_range_) || + ((p->orig_point->z - ground_cluster.getAverageHeight()) < minimum_detection_range_) || + (height_from_gnd > maximum_detection_range_)) { // exclude pcl under ground or higher than limit p->point_state = PointLabel::OUT_OF_RANGE; calculate_slope = false; From 1cf16029146f85e3195f4b71dd7038d4f6b5cc33 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 7 Jul 2022 10:25:22 +0900 Subject: [PATCH 6/9] docs: update parameters Signed-off-by: badai-nguyen --- perception/ground_segmentation/docs/scan-ground-filter.md | 2 ++ .../ground_segmentation/scan_ground_filter_nodelet.hpp | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index d854860b2c6c..2bbca70283c4 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -36,6 +36,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `radial_divider_angle` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | | `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to to distinguishing far and near [m] | | `split_height_distance` | double | 0.2 | The height threshold to distinguishing far and near [m] | +| `maximum_detection_range` | float | 2.5 | maximum of detection range from ground | +| `minimum_detection_range` | float | -0.2 | minimum of detection range from ground | | `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground 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 e612fbe44b46..de7ab67a95c3 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 @@ -114,8 +114,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter std::string base_frame_; std::string sensor_frame_; - float maximum_detection_range_ = 2.5f; // maximum height of non_ground object from ground - float minimum_detection_range_ = 0.0f; // mimimum height of non_ground object from ground + float maximum_detection_range_ = 2.5f; // maximum of detection range from ground + float minimum_detection_range_ = 0.0f; // minimum of detection range from ground double global_slope_max_angle_rad_; // radians double local_slope_max_angle_rad_; // radians double radial_divider_angle_rad_; // distance in rads between dividers From 73afe157d9a36a5797d85ceb5c7fcc1894997593 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 7 Jul 2022 10:47:53 +0900 Subject: [PATCH 7/9] fix: change range of crop_box_filter Signed-off-by: badai-nguyen --- .../ground_segmentation/ground_segmentation.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 654af3d9bfd0..f816f9aa924b 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -57,7 +57,7 @@ def get_vehicle_info(self): p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"] p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"]) p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"] - p["min_height_offset"] = 0.0 + p["min_height_offset"] = -0.5 p["max_height_offset"] = p["vehicle_height"] return p From 1065e40132f1c5c8bc4ecf78ebc80d7b47c1d3a9 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 7 Jul 2022 12:08:03 +0900 Subject: [PATCH 8/9] chore: add default params Signed-off-by: badai-nguyen --- .../ground_segmentation/ground_segmentation.param.yaml | 1 + .../ground_segmentation/ground_segmentation.launch.py | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index e3bef09e52b4..7cc665024456 100644 --- a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -20,4 +20,5 @@ local_slope_max_angle_deg: 30.0 split_points_distance_tolerance: 0.2 split_height_distance: 0.3 + minimum_detection_range: -0.2 use_virtual_ground_point: True diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index f816f9aa924b..cd2f47bc1262 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -103,7 +103,8 @@ def create_additional_pipeline(self, lidar_name): ("output", f"{lidar_name}/pointcloud"), ], parameters=[ - self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"] + {"maximum_detection_range": self.vehicle_info["max_height_offset"]}, + self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"], ], extra_arguments=[ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} @@ -237,6 +238,7 @@ def create_common_pipeline(self, input_topic, output_topic): ("output", output_topic), ], parameters=[ + {"maximum_detection_range": self.vehicle_info["max_height_offset"]}, self.ground_segmentation_param["common_ground_filter"]["parameters"], self.vehicle_info, ], From faab2b6d9ba468d7f03a7c3a824b5e437f736d15 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 8 Jul 2022 00:03:48 +0900 Subject: [PATCH 9/9] fix: recheck out of range and change default param Signed-off-by: badai-nguyen --- .../ground_segmentation/ground_segmentation.launch.py | 2 +- .../ground_segmentation/src/scan_ground_filter_nodelet.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index cd2f47bc1262..3dc41a0a0c4b 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -57,7 +57,7 @@ def get_vehicle_info(self): p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"] p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"]) p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"] - p["min_height_offset"] = -0.5 + p["min_height_offset"] = -2.0 p["max_height_offset"] = p["vehicle_height"] return p diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index a08cd9566671..278aa1b4f652 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -175,8 +175,9 @@ void ScanGroundFilterComponent::classifyPointCloud( float global_slope = std::atan2(p->orig_point->z, p->radius); // check points which is far enough from previous point if ( - (height_from_gnd < minimum_detection_range_) || - ((p->orig_point->z - ground_cluster.getAverageHeight()) < minimum_detection_range_) || + ((p->orig_point->z < 0.0) && + ((height_from_gnd < minimum_detection_range_) || + (p->orig_point->z - ground_cluster.getAverageHeight() < minimum_detection_range_))) || (height_from_gnd > maximum_detection_range_)) { // exclude pcl under ground or higher than limit p->point_state = PointLabel::OUT_OF_RANGE;