Skip to content

Commit

Permalink
feat(AEB): add height filter for avoiding the ghost brake by false po…
Browse files Browse the repository at this point in the history 
…sitive point clouds (#6637)

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

Signed-off-by: Shin-kyoto <[email protected]>

* docs(AEB): add param for height filter

Signed-off-by: Shin-kyoto <[email protected]>

* feat(AEB): add max height range in AEB

Signed-off-by: Shin-kyoto <[email protected]>

* feat(AEB): update default params

Signed-off-by: Shin-kyoto <[email protected]>

---------

Signed-off-by: Shin-kyoto <[email protected]>
  • Loading branch information
@Shin-kyoto
Shin-kyoto authored Mar 26, 2024
1 parent 106ee3e commit ce6de02
Show file tree
Hide file tree
Showing 4 changed files with 40 additions and 20 deletions.
40 changes: 21 additions & 19 deletions control/autonomous_emergency_braking/README.md
View file
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
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
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
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 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
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");
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 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
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");
height_filter.setFilterLimits(
detection_range_min_height_,
vehicle_info_.vehicle_height_m + detection_range_max_height_margin_);
height_filter.filter(*height_filtered_pointcloud_ptr);

pcl::VoxelGrid<pcl::PointXYZ> filter;
PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud);
filter.setInputCloud(pointcloud_ptr);
filter.setInputCloud(height_filtered_pointcloud_ptr);
filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_);
filter.filter(*no_height_filtered_pointcloud_ptr);

Expand Down

0 comments on commit ce6de02

Please sign in to comment.