Skip to content

Commit

Permalink
feat(AEB): implement parameterized prediction time horizon and interv…
Browse files Browse the repository at this point in the history 
…al (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>
  • Loading branch information
@1222-takeshi @pre-commit-ci @danielsanchezaran
2 people authored and danielsanchezaran committed Apr 15, 2024
1 parent 6987d6d commit ebdb1cf
Show file tree
Hide file tree
Showing 4 changed files with 43 additions and 26 deletions.
35 changes: 19 additions & 16 deletions control/autonomous_emergency_braking/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,22 +26,25 @@ This module has following assumptions.

## Parameters

| Name | Unit | Type | Description | Default value |
| :------------------------ | :----- | :----- | :-------------------------------------------------------------------------- | :------------ |
| 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 |
| prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 |
| prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 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 |
| 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
7 changes: 5 additions & 2 deletions control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
/**:
ros__parameters:
publish_debug_pointcloud: false
use_predicted_trajectory: true
use_imu_path: true
voxel_grid_x: 0.05
Expand All @@ -11,7 +12,9 @@
t_response: 1.0
a_ego_min: -3.0
a_obj_min: -1.0
prediction_time_horizon: 1.5
prediction_time_interval: 0.1
imu_prediction_time_horizon: 1.5
imu_prediction_time_interval: 0.1
mpc_prediction_time_horizon: 1.5
mpc_prediction_time_interval: 0.1
collision_keeping_sec: 0.0
aeb_hz: 10.0
7 changes: 5 additions & 2 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 @@ -172,6 +172,7 @@ class AEB : public rclcpp::Node
Updater updater_{this};

// member variables
bool publish_debug_pointcloud_;
bool use_predicted_trajectory_;
bool use_imu_path_;
double voxel_grid_x_;
Expand All @@ -183,8 +184,10 @@ class AEB : public rclcpp::Node
double t_response_;
double a_ego_min_;
double a_obj_min_;
double prediction_time_horizon_;
double prediction_time_interval_;
double imu_prediction_time_horizon_;
double imu_prediction_time_interval_;
double mpc_prediction_time_horizon_;
double mpc_prediction_time_interval_;
CollisionDataKeeper collision_data_keeper_;
};
} // namespace autoware::motion::control::autonomous_emergency_braking
Expand Down
20 changes: 14 additions & 6 deletions control/autonomous_emergency_braking/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision);

// parameter
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");
voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
Expand All @@ -127,8 +128,10 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
t_response_ = declare_parameter<double>("t_response");
a_ego_min_ = declare_parameter<double>("a_ego_min");
a_obj_min_ = declare_parameter<double>("a_obj_min");
prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
prediction_time_interval_ = declare_parameter<double>("prediction_time_interval");
imu_prediction_time_horizon_ = declare_parameter<double>("imu_prediction_time_horizon");
imu_prediction_time_interval_ = declare_parameter<double>("imu_prediction_time_interval");
mpc_prediction_time_horizon_ = declare_parameter<double>("mpc_prediction_time_horizon");
mpc_prediction_time_interval_ = declare_parameter<double>("mpc_prediction_time_interval");

const auto collision_keeping_sec = declare_parameter<double>("collision_keeping_sec");
collision_data_keeper_.setTimeout(collision_keeping_sec);
Expand Down Expand Up @@ -220,7 +223,9 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
obstacle_ros_pointcloud_ptr_->header = input_msg->header;
pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
if (publish_debug_pointcloud_) {
pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
}
}

bool AEB::isDataReady()
Expand Down Expand Up @@ -389,8 +394,8 @@ void AEB::generateEgoPath(
}

constexpr double epsilon = 1e-6;
const double & dt = prediction_time_interval_;
const double & horizon = prediction_time_horizon_;
const double & dt = imu_prediction_time_interval_;
const double & horizon = imu_prediction_time_horizon_;
for (double t = 0.0; t < horizon + epsilon; t += dt) {
curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt;
curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt;
Expand Down Expand Up @@ -449,8 +454,11 @@ void AEB::generateEgoPath(
geometry_msgs::msg::Pose map_pose;
tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped);
path.at(i) = map_pose;
}

if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) {
break;
}
}
// create polygon
polygons.resize(path.size());
for (size_t i = 0; i < path.size() - 1; ++i) {
Expand Down

0 comments on commit ebdb1cf

Please sign in to comment.