fix(behavior_path_planner_common): fix dynamic drivable area expansio…

…n with few input bound points (#8136)

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

planning/behavior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml

@@ -10,6 +10,7 @@
       enabled: true
       print_runtime: false
       max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit)
+      min_bound_interval: 0.1  # [m] minimum interval between two consecutive bound points (before expansion)
       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

planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -14,8 +14,6 @@
 
 #include "autoware/behavior_path_planner/behavior_path_planner_node.hpp"
 
-#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp"
-#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_utils.hpp"
 #include "autoware/motion_utils/trajectory/conversion.hpp"
 
@@ -910,6 +908,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
     updateParam(
       parameters, DrivableAreaExpansionParameters::SMOOTHING_ARC_LENGTH_RANGE_PARAM,
       planner_data_->drivable_area_expansion_parameters.arc_length_range);
+    updateParam(
+      parameters, DrivableAreaExpansionParameters::MIN_BOUND_INTERVAL,
+      planner_data_->drivable_area_expansion_parameters.min_bound_interval);
     updateParam(
       parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM,
       planner_data_->drivable_area_expansion_parameters.print_runtime);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp

@@ -58,6 +58,7 @@ struct DrivableAreaExpansionParameters
   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 constexpr auto MIN_BOUND_INTERVAL = "dynamic_expansion.min_bound_interval";
 
   // static expansion
   double drivable_area_right_bound_offset{};
@@ -80,6 +81,7 @@ struct DrivableAreaExpansionParameters
   double resample_interval{};
   double arc_length_range{};
   double max_reuse_deviation{};
+  double min_bound_interval{};
   bool avoid_dynamic_objects{};
   bool print_runtime{};
   std::vector<std::string> avoid_linestring_types{};
@@ -119,6 +121,7 @@ struct DrivableAreaExpansionParameters
       node.declare_parameter<std::vector<std::string>>(AVOID_LINESTRING_TYPES_PARAM);
     avoid_dynamic_objects = node.declare_parameter<bool>(AVOID_DYN_OBJECTS_PARAM);
     avoid_linestring_dist = node.declare_parameter<double>(AVOID_LINESTRING_DIST_PARAM);
+    min_bound_interval = node.declare_parameter<double>(MIN_BOUND_INTERVAL);
     print_runtime = node.declare_parameter<bool>(PRINT_RUNTIME_PARAM);
 
     vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();

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

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2023-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -23,6 +23,8 @@
 #include <autoware/motion_utils/resample/resample.hpp>
 #include <autoware/motion_utils/trajectory/interpolation.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <interpolation/linear_interpolation.hpp>
 
@@ -276,7 +278,13 @@ void expand_bound(
       const auto projection = point_to_linestring_projection(bound_p, path_ls);
       const auto expansion_ratio = (expansions[idx] + projection.distance) / projection.distance;
       const auto & path_p = projection.projected_point;
-      const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio);
+      auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio);
+      // push the bound again if it got too close to another part of the path
+      const auto new_projection = point_to_linestring_projection(expanded_p, path_ls);
+      if (new_projection.distance < projection.distance) {
+        const auto new_expansion_ratio = (projection.distance) / new_projection.distance;
+        expanded_p = lerp_point(new_projection.projected_point, expanded_p, new_expansion_ratio);
+      }
       bound[idx].x = expanded_p.x();
       bound[idx].y = expanded_p.y();
     }
@@ -291,8 +299,8 @@ void expand_bound(
         bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]);
       if (
         intersection &&
-        autoware::universe_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 &&
-        autoware::universe_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) {
+        autoware::universe_utils::calcDistance2d(*intersection, bound[idx - 1]) > 1e-3 &&
+        autoware::universe_utils::calcDistance2d(*intersection, bound[idx]) > 1e-3) {
         idx = succ_idx;
         is_intersecting = true;
       }
@@ -360,6 +368,42 @@ void calculate_expansion_distances(
   }
 }
 
+void add_bound_point(std::vector<Point> & bound, const Pose & pose, const double min_bound_interval)
+{
+  const auto p = convert_point(pose.position);
+  PointDistance nearest_projection;
+  nearest_projection.distance = std::numeric_limits<double>::infinity();
+  size_t nearest_idx = 0UL;
+  for (auto i = 0UL; i + 1 < bound.size(); ++i) {
+    const auto prev_p = convert_point(bound[i]);
+    const auto next_p = convert_point(bound[i + 1]);
+    const auto projection = point_to_segment_projection(p, prev_p, next_p);
+    if (projection.distance < nearest_projection.distance) {
+      nearest_projection = projection;
+      nearest_idx = i;
+    }
+  }
+  Point new_point;
+  new_point.x = nearest_projection.point.x();
+  new_point.y = nearest_projection.point.y();
+  new_point.z = bound[nearest_idx].z;
+  if (
+    universe_utils::calcDistance2d(new_point, bound[nearest_idx]) > min_bound_interval &&
+    universe_utils::calcDistance2d(new_point, bound[nearest_idx + 1]) > min_bound_interval) {
+    bound.insert(bound.begin() + nearest_idx + 1, new_point);
+  }
+}
+
+void add_bound_points(
+  std::vector<Point> & left_bound, std::vector<Point> & right_bound,
+  const std::vector<Pose> & path_poses, const double min_bound_interval)
+{
+  for (const auto & p : path_poses) {
+    add_bound_point(left_bound, p, min_bound_interval);
+    add_bound_point(right_bound, p, min_bound_interval);
+  }
+}
+
 void expand_drivable_area(
   PathWithLaneId & path,
   const std::shared_ptr<const autoware::behavior_path_planner::PlannerData> planner_data)
@@ -375,14 +419,14 @@ void expand_drivable_area(
     *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params);
   const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params);
   const auto preprocessing_ms = stop_watch.toc("preprocessing");
-
   stop_watch.tic("crop");
   std::vector<Pose> path_poses = planner_data->drivable_area_expansion_prev_path_poses;
   std::vector<double> curvatures = planner_data->drivable_area_expansion_prev_curvatures;
 
   reuse_previous_poses(
     path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params);
   const auto crop_ms = stop_watch.toc("crop");
+  add_bound_points(path.left_bound, path.right_bound, path_poses, params.min_bound_interval);
 
   stop_watch.tic("curvatures_expansion");
   // Only add curvatures for the new points. Curvatures of reused path points are not updated.

planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2023-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -264,13 +264,11 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area)
   autoware::behavior_path_planner::drivable_area_expansion::expand_drivable_area(
     path, std::make_shared<autoware::behavior_path_planner::PlannerData>(planner_data));
   // expanded left bound
-  ASSERT_EQ(path.left_bound.size(), 3ul);
-  EXPECT_GT(path.left_bound[0].y, 1.0);
-  EXPECT_GT(path.left_bound[1].y, 1.0);
-  EXPECT_GT(path.left_bound[2].y, 1.0);
+  for (const auto & p : path.left_bound) {
+    EXPECT_GT(p.y, 1.0);
+  }
   // expanded right bound
-  ASSERT_EQ(path.right_bound.size(), 3ul);
-  EXPECT_LT(path.right_bound[0].y, -1.0);
-  EXPECT_LT(path.right_bound[1].y, -1.0);
-  EXPECT_LT(path.right_bound[2].y, -1.0);
+  for (const auto & p : path.right_bound) {
+    EXPECT_LT(p.y, -1.0);
+  }
 }