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

feat(autonomous_emergency_braking): calculate the object's velocity in the search area #8591

Merged
merged 8 commits into from
Sep 26, 2024
+385 −89
Merged

feat(autonomous_emergency_braking): calculate the object's velocity in the search area #8591

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
f700faa
refactor PR
ismetatabay Sep 6, 2024
ff08509
WIP
ismetatabay Sep 6, 2024
6fb28f3
change using polygon to lateral offset
ismetatabay Sep 12, 2024
9f8e7b7
improve code
ismetatabay Sep 17, 2024
b58b691
remove redundant code
ismetatabay Sep 19, 2024
62ebc47
skip close points in MPC path generation
ismetatabay Sep 20, 2024
8879379
fix empty path points in short parking scenario
ismetatabay Sep 25, 2024
b49a8ae
fix readme conflicts
ismetatabay Sep 25, 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
65 changes: 37 additions & 28 deletions control/autoware_autonomous_emergency_braking/README.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading