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

feat(AEB): add height filter for avoiding the ghost brake by false positive point clouds #6637

Merged
merged 4 commits into from
Mar 26, 2024
Merged

feat(AEB): add height filter for avoiding the ghost brake by false positive point clouds #6637

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4989e3c
feat(AEB): add height filter for avoiding the ghost brake by false po…
Shin-kyoto Mar 15, 2024
27e5f8d
docs(AEB): add param for height filter
Shin-kyoto Mar 15, 2024
49311e4
feat(AEB): add max height range in AEB
Shin-kyoto Mar 16, 2024
4fbfcd0
feat(AEB): update default params
Shin-kyoto Mar 26, 2024
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
40 changes: 21 additions & 19 deletions control/autonomous_emergency_braking/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,25 +26,27 @@ This module has following assumptions.

## Parameters

| Name | Unit | Type | Description | Default value |
| :--------------------------- | :----- | :----- | :-------------------------------------------------------------------------- | :------------ |
| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false |
| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true |
| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true |
| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 |
| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 |
| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 |
| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 |
| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 |
| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 |
| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 |
| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 |
| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 |
| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 |
| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 |
| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 |
| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 |
| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 |
| Name | Unit | Type | Description | Default value |
| :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false |
| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true |
| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true |
| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 |
| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 |
| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 |
| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 |
| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 |
| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 |
| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 |
| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 |
| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 |
| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 |
| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 |
| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 |
| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 |
| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 |
| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 |
| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 |

## Inner-workings / Algorithms

Expand Down
2 changes: 2 additions & 0 deletions control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
publish_debug_pointcloud: false
use_predicted_trajectory: true
use_imu_path: true
detection_range_min_height: 0.0
detection_range_max_height_margin: 0.0
voxel_grid_x: 0.05
voxel_grid_y: 0.05
voxel_grid_z: 100000.0
Expand Down
2 changes: 2 additions & 0 deletions control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -175,6 +175,8 @@ class AEB : public rclcpp::Node
bool publish_debug_pointcloud_;
bool use_predicted_trajectory_;
bool use_imu_path_;
double detection_range_min_height_;
double detection_range_max_height_margin_;
double voxel_grid_x_;
double voxel_grid_y_;
double voxel_grid_z_;
Expand Down
16 changes: 15 additions & 1 deletion control/autonomous_emergency_braking/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include <boost/geometry/strategies/agnostic/hull_graham_andrew.hpp>

#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <tf2/utils.h>

Expand Down Expand Up @@ -128,6 +129,9 @@
publish_debug_pointcloud_ = declare_parameter<bool>("publish_debug_pointcloud");
use_predicted_trajectory_ = declare_parameter<bool>("use_predicted_trajectory");
use_imu_path_ = declare_parameter<bool>("use_imu_path");
detection_range_min_height_ = declare_parameter<double>("detection_range_min_height");
detection_range_max_height_margin_ =
declare_parameter<double>("detection_range_max_height_margin");

Check warning on line 134 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L132-L134 

Added lines #L132 - L134 were not covered by tests
voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
voxel_grid_y_ = declare_parameter<double>("voxel_grid_y");
voxel_grid_z_ = declare_parameter<double>("voxel_grid_z");
Expand Down Expand Up @@ -221,9 +225,19 @@
pcl::fromROSMsg(transformed_points, *pointcloud_ptr);
}

// apply z-axis filter for removing False Positive points
PointCloud::Ptr height_filtered_pointcloud_ptr(new PointCloud);
pcl::PassThrough<pcl::PointXYZ> height_filter;
height_filter.setInputCloud(pointcloud_ptr);
height_filter.setFilterFieldName("z");

Check warning on line 232 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L229-L232 

Added lines #L229 - L232 were not covered by tests
height_filter.setFilterLimits(
detection_range_min_height_,
vehicle_info_.vehicle_height_m + detection_range_max_height_margin_);
height_filter.filter(*height_filtered_pointcloud_ptr);

Check warning on line 236 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L234-L236 

Added lines #L234 - L236 were not covered by tests

pcl::VoxelGrid<pcl::PointXYZ> filter;
PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud);
filter.setInputCloud(pointcloud_ptr);
filter.setInputCloud(height_filtered_pointcloud_ptr);

Check warning on line 240 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L240 

Added line #L240 was not covered by tests
filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_);
filter.filter(*no_height_filtered_pointcloud_ptr);

Expand Down
Loading