diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index c3583982dde39..42b3f4c9f96de 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -213,7 +213,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | 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 | +| min_generated_imu_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| max_generated_imu_path_length | [m] | double | maximum distance for a predicted path generated by sensors | 10.0 | | 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 | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 75b54fe547e32..f7548536beaef 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -7,7 +7,8 @@ use_predicted_object_data: false use_object_velocity_calculation: true check_autoware_state: true - min_generated_path_length: 0.5 + min_generated_imu_path_length: 0.5 + max_generated_imu_path_length: 10.0 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 mpc_prediction_time_horizon: 4.5 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 6daa0a34dbaf4..214a6dc309210 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -562,7 +562,8 @@ class AEB : public rclcpp::Node double voxel_grid_x_; double voxel_grid_y_; double voxel_grid_z_; - double min_generated_path_length_; + double min_generated_imu_path_length_; + double max_generated_imu_path_length_; double expand_width_; double longitudinal_offset_; double t_response_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 34a7453b51726..b3988ba238fe7 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -165,7 +165,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) voxel_grid_x_ = declare_parameter("voxel_grid_x"); voxel_grid_y_ = declare_parameter("voxel_grid_y"); voxel_grid_z_ = declare_parameter("voxel_grid_z"); - min_generated_path_length_ = declare_parameter("min_generated_path_length"); + min_generated_imu_path_length_ = declare_parameter("min_generated_imu_path_length"); + max_generated_imu_path_length_ = declare_parameter("max_generated_imu_path_length"); expand_width_ = declare_parameter("expand_width"); longitudinal_offset_ = declare_parameter("longitudinal_offset"); t_response_ = declare_parameter("t_response"); @@ -227,7 +228,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "voxel_grid_x", voxel_grid_x_); updateParam(parameters, "voxel_grid_y", voxel_grid_y_); updateParam(parameters, "voxel_grid_z", voxel_grid_z_); - updateParam(parameters, "min_generated_path_length", min_generated_path_length_); + updateParam(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); + updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); updateParam(parameters, "expand_width", expand_width_); updateParam(parameters, "longitudinal_offset", longitudinal_offset_); updateParam(parameters, "t_response", t_response_); @@ -639,39 +641,35 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); - - if (std::abs(curr_v) < 0.1) { - // if current velocity is too small, assume it stops at the same point + const double & dt = imu_prediction_time_interval_; + const double distance_between_points = curr_v * dt; + constexpr double minimum_distance_between_points{1e-2}; + // if current velocity is too small, assume it stops at the same point + // if distance between points is too small, arc length calculation is unreliable, so we skip + // creating the path + if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) { return path; } - constexpr double epsilon = std::numeric_limits::epsilon(); - 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; - curr_yaw = curr_yaw + curr_w * dt; - geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { - continue; - } - path.push_back(current_pose); - } + double path_arc_length = 0.0; + double t = 0.0; - // If path is shorter than minimum path length - while (autoware::motion_utils::calcArcLength(path) < min_generated_path_length_) { + bool finished_creating_path = false; + while (!finished_creating_path) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { - continue; - } + + t += dt; + path_arc_length += distance_between_points; + + finished_creating_path = (t > horizon) && (path_arc_length > min_generated_imu_path_length_); + finished_creating_path = + (finished_creating_path) || (path_arc_length > max_generated_imu_path_length_); path.push_back(current_pose); } return path; @@ -691,12 +689,15 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) time_keeper_->start_track("createPath"); Path path; path.reserve(predicted_traj.points.size()); + constexpr double minimum_distance_between_points{1e-2}; for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); // skip points that are too close to the last point in the path - if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) { + if ( + autoware::universe_utils::calcDistance2d(path.back(), map_pose) < + minimum_distance_between_points) { continue; } diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 609a7a36b7d67..c2a58941cc144 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -171,7 +171,7 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) { const double dt = aeb_node_->imu_prediction_time_interval_; const double horizon = aeb_node_->imu_prediction_time_horizon_; - const double min_generated_path_length = aeb_node_->min_generated_path_length_; + const double min_generated_path_length = aeb_node_->min_generated_imu_path_length_; const double slow_velocity = min_generated_path_length / (2.0 * horizon); constexpr double yaw_rate = 0.05; const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); @@ -185,6 +185,26 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) ASSERT_TRUE(footprint.size() == imu_path.size() - 1); } +TEST_F(TestAEB, checkImuPathGenerationIsCut) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double max_generated_path_length = aeb_node_->max_generated_imu_path_length_; + const double fast_velocity = 2.0 * max_generated_path_length / (horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(fast_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + constexpr double epsilon{1e-3}; + ASSERT_TRUE( + autoware::motion_utils::calcArcLength(imu_path) <= + max_generated_path_length + dt * fast_velocity + epsilon); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) { const double velocity = 0.0;