Add smoothing.extra_arc_length param

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_path_planner/config/drivable_area_expansion.param.yaml

@@ -13,6 +13,7 @@
       smoothing:
         curvature_average_window: 3  # window size used for smoothing the curvatures using a moving window average
         max_bound_rate: 1.0  # [m/m] maximum rate of change of the bound lateral distance over its arc length
+        extra_arc_length: 2.0  # [m] extra arc length where an expansion distance is initially applied
       ego:
         extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase
         extra_front_overhang: 0.5 # [m] extra length to add to the front overhang

planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp

@@ -57,6 +57,8 @@ struct DrivableAreaExpansionParameters
     "dynamic_expansion.smoothing.curvature_average_window";
   static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM =
     "dynamic_expansion.smoothing.max_bound_rate";
+  static constexpr auto SMOOTHING_EXTRA_ARC_LENGTH_PARAM =
+    "dynamic_expansion.smoothing.extra_arc_length";
   static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime";
 
   // static expansion
@@ -78,6 +80,7 @@ struct DrivableAreaExpansionParameters
   double max_expansion_distance{};
   double max_path_arc_length{};
   double resample_interval{};
+  double extra_arc_length{};
   double max_reuse_deviation{};
   bool avoid_dynamic_objects{};
   bool print_runtime{};
@@ -102,6 +105,7 @@ struct DrivableAreaExpansionParameters
     extra_width = node.declare_parameter<double>(EGO_EXTRA_WIDTH);
     curvature_average_window = node.declare_parameter<int>(SMOOTHING_CURVATURE_WINDOW_PARAM);
     max_bound_rate = node.declare_parameter<double>(SMOOTHING_MAX_BOUND_RATE_PARAM);
+    extra_arc_length = node.declare_parameter<double>(SMOOTHING_EXTRA_ARC_LENGTH_PARAM);
     max_path_arc_length = node.declare_parameter<double>(MAX_PATH_ARC_LENGTH_PARAM);
     resample_interval = node.declare_parameter<double>(RESAMPLE_INTERVAL_PARAM);
     max_reuse_deviation = node.declare_parameter<double>(MAX_REUSE_DEVIATION_PARAM);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1051,6 +1051,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
     updated |= updateParam(
       parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM,
       planner_data_->drivable_area_expansion_parameters.max_bound_rate);
+    updated |= updateParam(
+      parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM,
+      planner_data_->drivable_area_expansion_parameters.extra_arc_length);
     updated |= updateParam(
       parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM,
       planner_data_->drivable_area_expansion_parameters.print_runtime);

planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -95,12 +95,12 @@ double calculate_minimum_lane_width(
   return (a * a + 2 * a * l + 2 * k * w + l * l + w * w) / (2 * k + w);
 }
 
-std::vector<BoundExpansion> calculate_minimum_expansions(
+std::vector<double> calculate_minimum_expansions(
   const std::vector<Pose> & path_poses, const std::vector<Point> bound,
   const std::vector<double> curvatures, const Side side,
   const DrivableAreaExpansionParameters & params)
 {
-  std::vector<BoundExpansion> bound_expansions(bound.size());
+  std::vector<double> minimum_expansions(bound.size());
   size_t lb_idx = 0;
   for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) {
     const auto & path_pose = path_poses[path_idx];
@@ -118,36 +118,52 @@ std::vector<BoundExpansion> calculate_minimum_expansions(
       if (intersection_point) {
         lb_idx = bound_idx;
         const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point);
-        if (dist > bound_expansions[bound_idx].expansion_distance) {
-          bound_expansions[bound_idx].expansion_distance = dist;
-          bound_expansions[bound_idx].expansion_point = offset_point;
-          bound_expansions[bound_idx].path_idx = path_idx;
-          bound_expansions[bound_idx].bound_segment_idx = bound_idx;
+        minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist);
+        minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist);
+        // apply the expansion to all bound points within the extra arc length
+        if (bound_idx + 2 < bound.size()) {
+          auto up_arc_length =
+            tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]) +
+            tier4_autoware_utils::calcDistance2d(bound[bound_idx + 1], bound[bound_idx + 2]);
+          for (auto up_bound_idx = bound_idx + 2;
+               bound_idx < bound.size() && up_arc_length <= params.extra_arc_length;
+               ++up_bound_idx) {
+            minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist);
+            if (up_bound_idx + 1 < bound.size())
+              up_arc_length +=
+                tier4_autoware_utils::calcDistance2d(bound[up_bound_idx], bound[up_bound_idx + 1]);
+          }
         }
-        if (dist > bound_expansions[bound_idx + 1].expansion_distance) {
-          bound_expansions[bound_idx + 1].expansion_distance = dist;
-          bound_expansions[bound_idx + 1].expansion_point = offset_point;
-          bound_expansions[bound_idx + 1].path_idx = path_idx;
-          bound_expansions[bound_idx + 1].bound_segment_idx = bound_idx;
+        if (bound_idx > 0) {
+          auto down_arc_length =
+            tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]) +
+            tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]);
+          for (auto down_bound_idx = bound_idx - 1;
+               down_bound_idx > 0 && down_arc_length <= params.extra_arc_length; --down_bound_idx) {
+            minimum_expansions[down_bound_idx] = std::max(minimum_expansions[down_bound_idx], dist);
+            if (down_bound_idx > 1)
+              down_arc_length += tier4_autoware_utils::calcDistance2d(
+                bound[down_bound_idx], bound[down_bound_idx - 1]);
+          }
         }
         break;
       }
     }
   }
-  return bound_expansions;
+  return minimum_expansions;
 }
 
 void apply_bound_velocity_limit(
-  std::vector<BoundExpansion> & expansions, const std::vector<Point> & bound_vector,
+  std::vector<double> & expansions, const std::vector<Point> & bound_vector,
   const double max_velocity)
 {
   if (expansions.empty()) return;
   const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) {
-    if (exp[from].expansion_distance > exp[to].expansion_distance) {
+    if (exp[from] > exp[to]) {
       const auto arc_length =
         tier4_autoware_utils::calcDistance2d(bound_vector[from], bound_vector[to]);
-      const auto smoothed_dist = exp[from].expansion_distance - arc_length * max_velocity;
-      exp[to].expansion_distance = std::max(exp[to].expansion_distance, smoothed_dist);
+      const auto smoothed_dist = exp[from] - arc_length * max_velocity;
+      exp[to] = std::max(exp[to], smoothed_dist);
     }
   };
   for (auto idx = 0LU; idx + 1 < expansions.size(); ++idx) apply_max_vel(expansions, idx, idx + 1);
@@ -187,16 +203,15 @@ std::vector<double> calculate_maximum_distance(
 
 void expand_bound(
   std::vector<Point> & bound, const std::vector<Pose> & path_poses,
-  const std::vector<BoundExpansion> & expansions)
+  const std::vector<double> & expansions)
 {
   LineString2d path_ls;
   for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position));
   for (auto idx = 0LU; idx < bound.size(); ++idx) {
     const auto bound_p = convert_point(bound[idx]);
     const auto projection = point_to_linestring_projection(bound_p, path_ls);
     const auto expansion_ratio =
-      (expansions[idx].expansion_distance + std::abs(projection.distance)) /
-      std::abs(projection.distance);
+      (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance);
     if (expansion_ratio > 1.0) {
       const auto & path_p = projection.projected_point;
       const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio);
@@ -281,11 +296,9 @@ void expand_drivable_area(
   const auto max_right_expansions = calculate_maximum_distance(
     path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons, params);
   for (auto i = 0LU; i < left_expansions.size(); ++i)
-    left_expansions[i].expansion_distance =
-      std::min(left_expansions[i].expansion_distance, max_left_expansions[i]);
+    left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]);
   for (auto i = 0LU; i < right_expansions.size(); ++i)
-    right_expansions[i].expansion_distance =
-      std::min(right_expansions[i].expansion_distance, max_right_expansions[i]);
+    right_expansions[i] = std::min(right_expansions[i], max_right_expansions[i]);
   const auto max_dist_ms = stop_watch.toc("max_dist");
 
   stop_watch.tic("smooth");