Skip to content

Commit ebdb1cf

Browse files
1222-takeshipre-commit-ci[bot]
authored and
danielsanchezaran
committed
feat(AEB): implement parameterized prediction time horizon and interval (autowarefoundation#5413)
* feat(AEB): implement parameterized prediction time horizon and interval Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com> * chore: update readme Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com> * style(pre-commit): autofix --------- Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 6987d6d commit ebdb1cf

File tree

4 files changed

+43
-26
lines changed

4 files changed

+43
-26
lines changed

‎control/autonomous_emergency_braking/README.md

+19-16
Original file line numberDiff line numberDiff line change
@@ -26,22 +26,25 @@ This module has following assumptions.
2626

2727
## Parameters
2828

29-
| Name | Unit | Type | Description | Default value |
30-
| :------------------------ | :----- | :----- | :-------------------------------------------------------------------------- | :------------ |
31-
| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true |
32-
| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true |
33-
| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 |
34-
| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 |
35-
| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 |
36-
| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 |
37-
| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 |
38-
| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 |
39-
| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 |
40-
| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 |
41-
| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 |
42-
| prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 |
43-
| prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 |
44-
| aeb_hz | [-] | double | 　frequency at which AEB operates per second | 10 |
29+
| Name | Unit | Type | Description | Default value |
30+
| :--------------------------- | :----- | :----- | :-------------------------------------------------------------------------- | :------------ |
31+
| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false |
32+
| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true |
33+
| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true |
34+
| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 |
35+
| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 |
36+
| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 |
37+
| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 |
38+
| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 |
39+
| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 |
40+
| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 |
41+
| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 |
42+
| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 |
43+
| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 |
44+
| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 |
45+
| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 |
46+
| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 |
47+
| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 |
4548

4649
## Inner-workings / Algorithms
4750

‎control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

+5-2
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
/**:
22
ros__parameters:
3+
publish_debug_pointcloud: false
34
use_predicted_trajectory: true
45
use_imu_path: true
56
voxel_grid_x: 0.05
@@ -11,7 +12,9 @@
1112
t_response: 1.0
1213
a_ego_min: -3.0
1314
a_obj_min: -1.0
14-
prediction_time_horizon: 1.5
15-
prediction_time_interval: 0.1
15+
imu_prediction_time_horizon: 1.5
16+
imu_prediction_time_interval: 0.1
17+
mpc_prediction_time_horizon: 1.5
18+
mpc_prediction_time_interval: 0.1
1619
collision_keeping_sec: 0.0
1720
aeb_hz: 10.0

‎control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -172,6 +172,7 @@ class AEB : public rclcpp::Node
172172
Updater updater_{this};
173173

174174
// member variables
175+
bool publish_debug_pointcloud_;
175176
bool use_predicted_trajectory_;
176177
bool use_imu_path_;
177178
double voxel_grid_x_;
@@ -183,8 +184,10 @@ class AEB : public rclcpp::Node
183184
double t_response_;
184185
double a_ego_min_;
185186
double a_obj_min_;
186-
double prediction_time_horizon_;
187-
double prediction_time_interval_;
187+
double imu_prediction_time_horizon_;
188+
double imu_prediction_time_interval_;
189+
double mpc_prediction_time_horizon_;
190+
double mpc_prediction_time_interval_;
188191
CollisionDataKeeper collision_data_keeper_;
189192
};
190193
} // namespace autoware::motion::control::autonomous_emergency_braking

‎control/autonomous_emergency_braking/src/node.cpp

+14-6
Original file line numberDiff line numberDiff line change
@@ -116,6 +116,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
116116
updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision);
117117

118118
// parameter
119+
publish_debug_pointcloud_ = declare_parameter<bool>("publish_debug_pointcloud");
119120
use_predicted_trajectory_ = declare_parameter<bool>("use_predicted_trajectory");
120121
use_imu_path_ = declare_parameter<bool>("use_imu_path");
121122
voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
@@ -127,8 +128,10 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
127128
t_response_ = declare_parameter<double>("t_response");
128129
a_ego_min_ = declare_parameter<double>("a_ego_min");
129130
a_obj_min_ = declare_parameter<double>("a_obj_min");
130-
prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
131-
prediction_time_interval_ = declare_parameter<double>("prediction_time_interval");
131+
imu_prediction_time_horizon_ = declare_parameter<double>("imu_prediction_time_horizon");
132+
imu_prediction_time_interval_ = declare_parameter<double>("imu_prediction_time_interval");
133+
mpc_prediction_time_horizon_ = declare_parameter<double>("mpc_prediction_time_horizon");
134+
mpc_prediction_time_interval_ = declare_parameter<double>("mpc_prediction_time_interval");
132135

133136
const auto collision_keeping_sec = declare_parameter<double>("collision_keeping_sec");
134137
collision_data_keeper_.setTimeout(collision_keeping_sec);
@@ -220,7 +223,9 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
220223
obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
221224
pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
222225
obstacle_ros_pointcloud_ptr_->header = input_msg->header;
223-
pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
226+
if (publish_debug_pointcloud_) {
227+
pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
228+
}
224229
}
225230

226231
bool AEB::isDataReady()
@@ -389,8 +394,8 @@ void AEB::generateEgoPath(
389394
}
390395

391396
constexpr double epsilon = 1e-6;
392-
const double & dt = prediction_time_interval_;
393-
const double & horizon = prediction_time_horizon_;
397+
const double & dt = imu_prediction_time_interval_;
398+
const double & horizon = imu_prediction_time_horizon_;
394399
for (double t = 0.0; t < horizon + epsilon; t += dt) {
395400
curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt;
396401
curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt;
@@ -449,8 +454,11 @@ void AEB::generateEgoPath(
449454
geometry_msgs::msg::Pose map_pose;
450455
tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped);
451456
path.at(i) = map_pose;
452-
}
453457

458+
if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) {
459+
break;
460+
}
461+
}
454462
// create polygon
455463
polygons.resize(path.size());
456464
for (size_t i = 0; i < path.size() - 1; ++i) {

0 commit comments

Comments
 (0)