feat(drivable area expansion): do not over expand beyond the desired …

…width (#5640)

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

@@ -35,14 +35,14 @@ void expand_drivable_area(
   PathWithLaneId & path,
   const std::shared_ptr<const behavior_path_planner::PlannerData> planner_data);
 
-/// @brief prepare path poses and try to reuse their previously calculated curvatures
+/// @brief try to reuse the previous path poses and their previously calculated curvatures
 /// @details poses are reused if they do not deviate too much from the current path
 /// @param [in] path input path
 /// @param [inout] prev_poses previous poses to reuse
 /// @param [inout] prev_curvatures previous curvatures to reuse
 /// @param [in] ego_point current ego point
 /// @param [in] params parameters for reuse criteria and resampling interval
-void update_path_poses_and_previous_curvatures(
+void reuse_previous_poses(
   const PathWithLaneId & path, std::vector<Pose> & prev_poses,
   std::vector<double> & prev_curvatures, const Point & ego_point,
   const DrivableAreaExpansionParameters & params);
@@ -57,6 +57,36 @@ void update_path_poses_and_previous_curvatures(
 double calculate_minimum_lane_width(
   const double curvature_radius, const DrivableAreaExpansionParameters & params);
 
+/// @brief calculate mappings between path poses and the given drivable area bound
+/// @param [inout] expansion expansion data to update with the mapping
+/// @param [in] path_poses path poses
+/// @param [in] bound drivable area bound
+/// @param [in] Side left or right side
+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 range
+/// @param [inout] expansion expansion data to update
+/// @param [in] bound drivable area bound
+/// @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_arc_length_range_smoothing(
+  Expansion & expansion, const std::vector<Point> & bound, const double arc_length_range,
+  const Side side);
+
+/// @brief calculate minimum lane widths and mappings between path and and drivable area bounds
+/// @param [in] path_poses path poses
+/// @param [in] left_bound left drivable area bound
+/// @param [in] right_bound right drivable area bound
+/// @param [in] curvatures curvatures at each path point
+/// @param [in] params parameters with the vehicle dimensions used to calculate the min lane width
+/// @return expansion data (path->bound mappings, min lane widths, ...)
+Expansion calculate_expansion(
+  const std::vector<Pose> & path_poses, const std::vector<Point> & left_bound,
+  const std::vector<Point> & right_bound, const std::vector<double> & curvatures,
+  const DrivableAreaExpansionParameters & params);
+
 /// @brief smooth the bound by applying a limit on its rate of change
 /// @details rate of change is the lateral distance from the path over the arc length along the path
 /// @param [inout] bound_distances bound distances (lateral distance from the path)
@@ -66,16 +96,16 @@ void apply_bound_change_rate_limit(
   std::vector<double> & distances, const std::vector<Point> & bound, const double max_rate);
 
 /// @brief calculate the maximum distance by which a bound can be expanded
-/// @param [in] path_poses input path
 /// @param [in] bound bound points
 /// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree
 /// @param [in] uncrossable_polygons polygons that limit the bound expansion
 /// @param [in] params parameters with the buffer distance to keep with lines,
 /// and the static maximum expansion distance
+/// @param [in] Side left or right side
 std::vector<double> calculate_maximum_distance(
-  const std::vector<Pose> & path_poses, const std::vector<Point> bound,
-  const SegmentRtree & uncrossable_lines, const std::vector<Polygon2d> & uncrossable_polygons,
-  const DrivableAreaExpansionParameters & params);
+  const std::vector<Point> & bound, const SegmentRtree & uncrossable_lines,
+  const std::vector<Polygon2d> & uncrossable_polygons,
+  const DrivableAreaExpansionParameters & params, const Side side);
 
 /// @brief expand a bound by the given lateral distances away from the path
 /// @param [inout] bound bound points to expand
@@ -85,6 +115,14 @@ void expand_bound(
   std::vector<Point> & bound, const std::vector<Pose> & path_poses,
   const std::vector<double> & distances);
 
+/// @brief calculate the expansion distances of the left and right drivable area bounds
+/// @param [inout] expansion expansion data to be updated with the left/right expansion distances
+/// @param [in] max_left_expansions maximum left expansion distances
+/// @param [in] max_right_expansions maximum right expansion distances
+void calculate_expansion_distances(
+  Expansion & expansion, const std::vector<double> & max_left_expansions,
+  const std::vector<double> & max_right_expansions);
+
 /// @brief calculate smoothed curvatures
 /// @details smoothing is done using a moving average
 /// @param [in] poses input poses used to calculate the curvatures

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/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp

@@ -26,7 +26,6 @@
 namespace drivable_area_expansion
 {
 /// @brief project a point to a segment
-/// @details the distance is signed based on the side of the point: left=positive, right=negative
 /// @param p point to project on the segment
 /// @param p1 first segment point
 /// @param p2 second segment point
@@ -37,22 +36,18 @@ inline PointDistance point_to_segment_projection(
   const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()};
   const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()};
 
-  const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x();
-  const auto dist_sign = cross < 0.0 ? -1.0 : 1.0;
-
   const auto c1 = boost::geometry::dot_product(p_vec, p2_vec);
-  if (c1 <= 0) return {p1, boost::geometry::distance(p, p1) * dist_sign};
+  if (c1 <= 0) return {p1, boost::geometry::distance(p, p1)};
 
   const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec);
-  if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign};
+  if (c2 <= c1) return {p2, boost::geometry::distance(p, p2)};
 
   const auto projection = p1 + (p2_vec * c1 / c2);
   const auto projection_point = Point2d{projection.x(), projection.y()};
-  return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign};
+  return {projection_point, boost::geometry::distance(p, projection_point)};
 }
 
 /// @brief project a point to a line
-/// @details the distance is signed based on the side of the point: left=positive, right=negative
 /// @param p point to project on the line
 /// @param p1 first line point
 /// @param p2 second line point
@@ -63,14 +58,11 @@ inline PointDistance point_to_line_projection(
   const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()};
   const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()};
 
-  const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x();
-  const auto dist_sign = cross < 0.0 ? -1.0 : 1.0;
-
   const auto c1 = boost::geometry::dot_product(p_vec, p2_vec);
   const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec);
   const auto projection = p1 + (p2_vec * c1 / c2);
   const auto projection_point = Point2d{projection.x(), projection.y()};
-  return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign};
+  return {projection_point, boost::geometry::distance(p, projection_point)};
 }
 
 /// @brief project a point to a linestring
@@ -84,7 +76,7 @@ inline Projection point_to_linestring_projection(const Point2d & p, const LineSt
   auto arc_length = 0.0;
   for (auto ls_it = ls.begin(); std::next(ls_it) != ls.end(); ++ls_it) {
     const auto pd = point_to_segment_projection(p, *ls_it, *(ls_it + 1));
-    if (std::abs(pd.distance) < std::abs(closest.distance)) {
+    if (pd.distance < closest.distance) {
       closest.projected_point = pd.point;
       closest.distance = pd.distance;
       closest.arc_length = arc_length + boost::geometry::distance(*ls_it, pd.point);

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

@@ -24,6 +24,9 @@
 
 #include <boost/geometry/index/rtree.hpp>
 
+#include <optional>
+#include <vector>
+
 namespace drivable_area_expansion
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
@@ -45,16 +48,27 @@ using SegmentRtree = boost::geometry::index::rtree<Segment2d, boost::geometry::i
 
 struct PointDistance
 {
-  Point2d point;
-  double distance{0.0};
+  Point2d point{};
+  double distance{};
 };
 struct Projection
 {
-  Point2d projected_point;
-  double distance{0.0};
-  double arc_length{0.0};
+  Point2d projected_point{};
+  double distance{};
+  double arc_length{};
 };
 enum Side { LEFT, RIGHT };
-
+struct Expansion
+{
+  // mappings from bound index
+  std::vector<double> left_distances;
+  std::vector<double> right_distances;
+  // mappings from path index
+  std::vector<size_t> left_bound_indexes;
+  std::vector<PointDistance> left_projections;
+  std::vector<size_t> right_bound_indexes;
+  std::vector<PointDistance> right_projections;
+  std::vector<double> min_lane_widths;
+};
 }  // namespace drivable_area_expansion
 #endif  // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_