feat(autonomous_emergency_braking): set max imu path length (#9004)

* set a limit to the imu path length Signed-off-by: Daniel Sanchez <[email protected]> * fix test and add a new one Signed-off-by: Daniel Sanchez <[email protected]> * update readme Signed-off-by: Daniel Sanchez <[email protected]> * pre-commit Signed-off-by: Daniel Sanchez <[email protected]> * use velocity and time directly to get arc length Signed-off-by: Daniel Sanchez <[email protected]> * refactor to reduce repeated code Signed-off-by: Daniel Sanchez <[email protected]> * cleaning code Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
autowarefoundation · Oct 2, 2024 · 5b8219e · 5b8219e
1 parent 2b179f4
commit 5b8219e
Show file tree

Hide file tree

Showing 5 changed files with 53 additions and 29 deletions.
diff --git 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
@@ -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/...oware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/...oware_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
@@ -165,7 +165,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
   voxel_grid_y_ = declare_parameter<double>("voxel_grid_y");
   voxel_grid_z_ = declare_parameter<double>("voxel_grid_z");
-  min_generated_path_length_ = declare_parameter<double>("min_generated_path_length");
+  min_generated_imu_path_length_ = declare_parameter<double>("min_generated_imu_path_length");
+  max_generated_imu_path_length_ = declare_parameter<double>("max_generated_imu_path_length");
   expand_width_ = declare_parameter<double>("expand_width");
   longitudinal_offset_ = declare_parameter<double>("longitudinal_offset");
   t_response_ = declare_parameter<double>("t_response");
@@ -227,7 +228,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter(
   updateParam<double>(parameters, "voxel_grid_x", voxel_grid_x_);
   updateParam<double>(parameters, "voxel_grid_y", voxel_grid_y_);
   updateParam<double>(parameters, "voxel_grid_z", voxel_grid_z_);
-  updateParam<double>(parameters, "min_generated_path_length", min_generated_path_length_);
+  updateParam<double>(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_);
+  updateParam<double>(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_);
   updateParam<double>(parameters, "expand_width", expand_width_);
   updateParam<double>(parameters, "longitudinal_offset", longitudinal_offset_);
   updateParam<double>(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<double>::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<Path> 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
@@ -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;