Skip to content

Commit 45792df

Browse files
1222-takeshipre-commit-ci[bot]
1222-takeshi
and
pre-commit-ci[bot]
authored
feat(AEB): implement parameterized prediction time horizon and interval (#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 fdd671f commit 45792df

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
@@ -125,6 +125,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
125125
updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision);
126126

127127
// parameter
128+
publish_debug_pointcloud_ = declare_parameter<bool>("publish_debug_pointcloud");
128129
use_predicted_trajectory_ = declare_parameter<bool>("use_predicted_trajectory");
129130
use_imu_path_ = declare_parameter<bool>("use_imu_path");
130131
voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
@@ -136,8 +137,10 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
136137
t_response_ = declare_parameter<double>("t_response");
137138
a_ego_min_ = declare_parameter<double>("a_ego_min");
138139
a_obj_min_ = declare_parameter<double>("a_obj_min");
139-
prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
140-
prediction_time_interval_ = declare_parameter<double>("prediction_time_interval");
140+
imu_prediction_time_horizon_ = declare_parameter<double>("imu_prediction_time_horizon");
141+
imu_prediction_time_interval_ = declare_parameter<double>("imu_prediction_time_interval");
142+
mpc_prediction_time_horizon_ = declare_parameter<double>("mpc_prediction_time_horizon");
143+
mpc_prediction_time_interval_ = declare_parameter<double>("mpc_prediction_time_interval");
141144

142145
const auto collision_keeping_sec = declare_parameter<double>("collision_keeping_sec");
143146
collision_data_keeper_.setTimeout(collision_keeping_sec);
@@ -227,7 +230,9 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
227230
obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
228231
pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
229232
obstacle_ros_pointcloud_ptr_->header = input_msg->header;
230-
pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
233+
if (publish_debug_pointcloud_) {
234+
pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
235+
}
231236
}
232237

233238
bool AEB::isDataReady()
@@ -392,8 +397,8 @@ void AEB::generateEgoPath(
392397
}
393398

394399
constexpr double epsilon = 1e-6;
395-
const double & dt = prediction_time_interval_;
396-
const double & horizon = prediction_time_horizon_;
400+
const double & dt = imu_prediction_time_interval_;
401+
const double & horizon = imu_prediction_time_horizon_;
397402
for (double t = 0.0; t < horizon + epsilon; t += dt) {
398403
curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt;
399404
curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt;
@@ -452,8 +457,11 @@ void AEB::generateEgoPath(
452457
geometry_msgs::msg::Pose map_pose;
453458
tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped);
454459
path.at(i) = map_pose;
455-
}
456460

461+
if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) {
462+
break;
463+
}
464+
}
457465
// create polygon
458466
polygons.resize(path.size());
459467
for (size_t i = 0; i < path.size() - 1; ++i) {

0 commit comments

Comments
 (0)