Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(ground_filter): expand range of cropped_box_filter #1241

Closed
wants to merge 9 commits into from
Closed

fix(ground_filter): expand range of cropped_box_filter #1241

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
86e547c
fix(ground_filter): expand range of cropped_box_filter
badai-nguyen Jul 4, 2022
5a67792
chore: typo
badai-nguyen Jul 6, 2022
d79d3fe
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 6, 2022
23d85f5
chore: add out_range point label
badai-nguyen Jul 7, 2022
c786f6b
chore: typo
badai-nguyen Jul 7, 2022
1cf1602
docs: update parameters
badai-nguyen Jul 7, 2022
73afe15
fix: change range of crop_box_filter
badai-nguyen Jul 7, 2022
1065e40
chore: add default params
badai-nguyen Jul 7, 2022
faab2b6
fix: recheck out of range and change default param
badai-nguyen Jul 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ...on_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
6 changes: 4 additions & 2 deletions ...ion_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"] = -2.0
p["max_height_offset"] = p["vehicle_height"]
return p

Expand Down Expand Up @@ -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")}
Expand Down Expand Up @@ -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,
],
Expand Down
2 changes: 2 additions & 0 deletions perception/ground_segmentation/docs/scan-ground-filter.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 3 additions & 0 deletions perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
POINT_FOLLOW,
UNKNOWN,
VIRTUAL_GROUND,
OUT_OF_RANGE
};
struct PointRef
{
Expand Down Expand Up @@ -113,6 +114,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter

std::string base_frame_;
std::string sensor_frame_;
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
Expand Down
16 changes: 15 additions & 1 deletion perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
// set initial parameters
{
base_frame_ = declare_parameter("base_frame", "base_link");
maximum_detection_range_ =
static_cast<float>(declare_parameter("maximum_detection_range", 2.5));
minimum_detection_range_ =
static_cast<float>(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));
radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0));
Expand Down Expand Up @@ -168,7 +174,15 @@ 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 (
((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;
calculate_slope = false;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In this case, what would be the label for point?

Copy link
Contributor Author

@badai-nguyen badai-nguyen Jul 7, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I assigned as out_of_range label for that points

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

When the point is labeled out of range, prev point label will be also set to out of range. Is this okay for next point judgement?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

as this condition , if the point is labeled out of range, the next point should not classify as POINT_FOLLOW, but GROUND, NON_GROUND or OUT_OF_RANGE.
Keeping to use the current ground_cluster and non_ground_cluster to check the next point is reasonable, IMO.

} else if (global_slope > global_slope_max_angle) {
p->point_state = PointLabel::NON_GROUND;
calculate_slope = false;
} else if (
Expand Down