Simplify and rename function to smooth the expansion over an arc leng…

…th range

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

planning/behavior_path_planner/config/drivable_area_expansion.param.yaml

@@ -13,7 +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
+        arc_length_range: 2.0  # [m] arc length range 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/src/behavior_path_planner_node.cpp

@@ -929,8 +929,8 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
       parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM,
       planner_data_->drivable_area_expansion_parameters.max_bound_rate);
     updateParam(
-      parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM,
-      planner_data_->drivable_area_expansion_parameters.extra_arc_length);
+      parameters, DrivableAreaExpansionParameters::SMOOTHING_ARC_LENGTH_RANGE_PARAM,
+      planner_data_->drivable_area_expansion_parameters.arc_length_range);
     updateParam(
       parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM,
       planner_data_->drivable_area_expansion_parameters.print_runtime);

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp

@@ -66,13 +66,13 @@ void calculate_bound_index_mappings(
   Expansion & expansion, const std::vector<Pose> & path_poses, const std::vector<Point> & bound,
   const Side side);
 
-/// @brief apply expansion distances to all bound points within the given arc length
+/// @brief apply expansion distances to all bound points within the given arc length range
 /// @param [inout] expansion expansion data to update
 /// @param [in] bound drivable area bound
-/// @param [in] extra_arc_length [m] extra arc length to apply around each expansion
+/// @param [in] arc_length_range [m] arc length range where the expansion distances are also applied
 /// @param [in] Side left or right side
-void apply_extra_arc_length(
-  Expansion & expansion, const std::vector<Point> & bound, const double extra_arc_length,
+void apply_arc_length_range_smoothing(
+  Expansion & expansion, const std::vector<Point> & bound, const double arc_length_range,
   const Side side);
 
 /// @brief calculate mappings between path poses and the given drivable area bound

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

@@ -55,8 +55,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 SMOOTHING_ARC_LENGTH_RANGE_PARAM =
+    "dynamic_expansion.smoothing.arc_length_range";
   static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime";
 
   // static expansion
@@ -78,7 +78,7 @@ struct DrivableAreaExpansionParameters
   double max_expansion_distance{};
   double max_path_arc_length{};
   double resample_interval{};
-  double extra_arc_length{};
+  double arc_length_range{};
   double max_reuse_deviation{};
   bool avoid_dynamic_objects{};
   bool print_runtime{};
@@ -103,7 +103,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);
+    arc_length_range = node.declare_parameter<double>(SMOOTHING_ARC_LENGTH_RANGE_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_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -146,8 +146,8 @@ void calculate_bound_index_mappings(
   }
 }
 
-void apply_extra_arc_length(
-  Expansion & expansion, const std::vector<Point> & bound, const double extra_arc_length,
+void apply_arc_length_range_smoothing(
+  Expansion & expansion, const std::vector<Point> & bound, const double arc_length_range,
   const Side side)
 {
   const auto & bound_indexes =
@@ -160,20 +160,19 @@ void apply_extra_arc_length(
     const auto bound_idx = bound_indexes[path_idx];
     auto arc_length = boost::geometry::distance(
       bound_projections[path_idx].point, convert_point(bound[bound_idx + 1]));
+    const auto update_arc_length_and_bound_expansions = [&](auto idx) {
+      arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]);
+      bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]);
+    };
     for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) {
-      arc_length +=
-        tier4_autoware_utils::calcDistance2d(bound[up_bound_idx - 1], bound[up_bound_idx]);
-      if (arc_length > extra_arc_length) break;
-      bound_expansions[up_bound_idx] =
-        std::max(bound_expansions[up_bound_idx], original_expansions[bound_idx]);
+      update_arc_length_and_bound_expansions(up_bound_idx);
+      if (arc_length > arc_length_range) break;
     }
     arc_length =
       boost::geometry::distance(bound_projections[path_idx].point, convert_point(bound[bound_idx]));
     for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) {
-      const auto idx = bound_idx - down_offset_idx;
-      arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]);
-      if (arc_length > extra_arc_length) break;
-      bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]);
+      update_arc_length_and_bound_expansions(bound_idx - down_offset_idx);
+      if (arc_length > arc_length_range) break;
     }
   }
 }
@@ -401,8 +400,8 @@ void expand_drivable_area(
   const auto max_dist_ms = stop_watch.toc("max_dist");
 
   calculate_expansion_distances(expansion, max_left_expansions, max_right_expansions);
-  apply_extra_arc_length(expansion, path.left_bound, params.extra_arc_length, LEFT);
-  apply_extra_arc_length(expansion, path.right_bound, params.extra_arc_length, RIGHT);
+  apply_arc_length_range_smoothing(expansion, path.left_bound, params.arc_length_range, LEFT);
+  apply_arc_length_range_smoothing(expansion, path.right_bound, params.arc_length_range, RIGHT);
 
   stop_watch.tic("smooth");
   apply_bound_change_rate_limit(expansion.left_distances, path.left_bound, params.max_bound_rate);