Skip to content

Commit

Permalink
feat(autonomous_emergency_braking): calculate the object's velocity i…
Browse files Browse the repository at this point in the history 
…n the search area (#8591)

* refactor PR

Signed-off-by: ismetatabay <[email protected]>

* WIP

Signed-off-by: ismetatabay <[email protected]>

* change using polygon to lateral offset

Signed-off-by: ismetatabay <[email protected]>

* improve code

Signed-off-by: ismetatabay <[email protected]>

* remove redundant code

Signed-off-by: ismetatabay <[email protected]>

* skip close points in MPC path generation

Signed-off-by: ismetatabay <[email protected]>

* fix empty path points in short parking scenario

Signed-off-by: ismetatabay <[email protected]>

* fix readme conflicts

Signed-off-by: ismetatabay <[email protected]>

---------

Signed-off-by: ismetatabay <[email protected]>
  • Loading branch information
@ismetatabay
ismetatabay authored Sep 26, 2024
1 parent dda2f19 commit 8f0e511
Show file tree
Hide file tree
Showing 7 changed files with 385 additions and 89 deletions.
65 changes: 37 additions & 28 deletions control/autoware_autonomous_emergency_braking/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,14 @@ Once all target obstacles have been identified, the AEB module chooses the point

### 4. Obstacle velocity estimation

To begin calculating the target point's velocity, the point must enter the speed calculation area,
which is defined by the `speed_calculation_expansion_margin` parameter.
Depending on the operational environment,
this margin can reduce unnecessary autonomous emergency braking
caused by velocity miscalculations during the initial calculation steps.

![speed_calculation_expansion](./image/speed_calculation_expansion.drawio.svg)

Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object relative speed using the following equations:

$$
Expand Down Expand Up @@ -188,34 +196,35 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t

## Parameters

| Name | Unit | Type | Description | Default value |
| :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| publish_debug_markers | [-] | bool | flag to publish debug markers | true |
| 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 |
| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true |
| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | 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 |
| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 |
| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 |
| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 |
| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 |
| 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_markers | [-] | bool | flag to publish debug markers | true |
| 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 |
| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true |
| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | 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 |
| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 |
| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 |
| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 |
| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 |
| 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 |
| speed_calculation_expansion_margin | [m] | double | expansion width of the ego vehicle for the beginning speed calculation | 0.1 |

## Limitations

Expand Down
1 change: 1 addition & 0 deletions control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
# Point cloud cropping
expand_width: 0.1
path_footprint_extra_margin: 4.0
speed_calculation_expansion_margin: 0.5

# Point cloud clustering
cluster_tolerance: 0.15 #[m]
Expand Down
234 changes: 234 additions & 0 deletions ...oware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
8 changes: 7 additions & 1 deletion ...oware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tier4_debug_msgs/msg/float32_stamped.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

Expand Down Expand Up @@ -87,6 +88,7 @@ struct ObjectData
double velocity{0.0};
double rss{0.0};
double distance_to_object{0.0};
bool is_target{true};
};

/**
Expand Down Expand Up @@ -342,6 +344,7 @@ class AEB : public rclcpp::Node
rclcpp::Publisher<MarkerArray>::SharedPtr info_marker_publisher_;
rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
debug_processing_time_detail_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr debug_rss_distance_publisher_;
// timer
rclcpp::TimerBase::SharedPtr timer_;
mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};
Expand Down Expand Up @@ -436,12 +439,14 @@ class AEB : public rclcpp::Node
* @brief Create object data using point cloud clusters
* @param ego_path Ego vehicle path
* @param ego_polys Polygons representing the ego vehicle footprint
* @param speed_calc_ego_polys Polygons representing the expanded ego vehicle footprint for speed
* calculation area
* @param stamp Timestamp of the data
* @param objects Vector to store the created object data
* @param obstacle_points_ptr Pointer to the point cloud of obstacles
*/
void getClosestObjectsOnPath(
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
const Path & ego_path, const rclcpp::Time & stamp,
const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector<ObjectData> & objects);

/**
Expand Down Expand Up @@ -551,6 +556,7 @@ class AEB : public rclcpp::Node
bool use_object_velocity_calculation_;
bool check_autoware_state_;
double path_footprint_extra_margin_;
double speed_calculation_expansion_margin_;
double detection_range_min_height_;
double detection_range_max_height_margin_;
double voxel_grid_x_;
Expand Down
1 change: 1 addition & 0 deletions control/autoware_autonomous_emergency_braking/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>
<depend>visualization_msgs</depend>

<export>
Expand Down
Loading

0 comments on commit 8f0e511

Please sign in to comment.