fix(behavior_path_planner): fix bad dynamic drivable area expansion a…

…t path extremities (autowarefoundation#4259) (#721)

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

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

@@ -40,7 +40,7 @@ void expandDrivableArea(
 /// @param[in] expansion_polygons polygons to add to the drivable area
 /// @return expanded drivable area polygon
 polygon_t createExpandedDrivableAreaPolygon(
-  const PathWithLaneId & path, const multipolygon_t & expansion_polygons);
+  const PathWithLaneId & path, const multi_polygon_t & expansion_polygons);
 
 /// @brief Update the drivable area of the given path with the given polygon
 /// @details this function splits the polygon into a left and right bound and sets it in the path

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

@@ -36,7 +36,7 @@ namespace drivable_area_expansion
 /// @return distance limit
 double calculateDistanceLimit(
   const linestring_t & base_ls, const polygon_t & expansion_polygon,
-  const multilinestring_t & limit_lines);
+  const multi_linestring_t & limit_lines);
 
 /// @brief Calculate the distance limit required for the polygon to not cross the limit polygons.
 /// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and
@@ -47,7 +47,7 @@ double calculateDistanceLimit(
 /// @return distance limit
 double calculateDistanceLimit(
   const linestring_t & base_ls, const polygon_t & expansion_polygon,
-  const multipolygon_t & limit_polygons);
+  const multi_polygon_t & limit_polygons);
 
 /// @brief Create a polygon from a base line with a given expansion distance
 /// @param[in] base_ls base linestring from which the polygon is created
@@ -64,9 +64,9 @@ polygon_t createExpansionPolygon(
 /// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area
 /// @param[in] params expansion parameters
 /// @return expansion polygons
-multipolygon_t createExpansionPolygons(
-  const PathWithLaneId & path, const multipolygon_t & path_footprints,
-  const multipolygon_t & predicted_paths, const multilinestring_t & uncrossable_lines,
+multi_polygon_t createExpansionPolygons(
+  const PathWithLaneId & path, const multi_polygon_t & path_footprints,
+  const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines,
   const DrivableAreaExpansionParameters & params);
 
 /// @brief Create polygons for the area where the drivable area should be expanded
@@ -76,9 +76,9 @@ multipolygon_t createExpansionPolygons(
 /// @param[in] predicted_paths polygons of the dynamic objects' predicted paths
 /// @param[in] params expansion parameters
 /// @return expansion polygons
-multipolygon_t createExpansionLaneletPolygons(
+multi_polygon_t createExpansionLaneletPolygons(
   const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler,
-  const multipolygon_t & path_footprints, const multipolygon_t & predicted_paths,
+  const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths,
   const DrivableAreaExpansionParameters & params);
 }  // namespace drivable_area_expansion
 

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

@@ -55,15 +55,15 @@ polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t
 /// @param [in] objects objects from which to create polygons
 /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects
 /// @return footprint polygons of the object's predicted paths
-multipolygon_t createObjectFootprints(
+multi_polygon_t createObjectFootprints(
   const autoware_auto_perception_msgs::msg::PredictedObjects & objects,
   const DrivableAreaExpansionParameters & params);
 
 /// @brief create the footprint polygon from a path
 /// @param[in] path the path for which to create a footprint
 /// @param[in] params expansion parameters defining how to create the footprint
 /// @return footprint polygons of the path
-multipolygon_t createPathFootprints(
+multi_polygon_t createPathFootprints(
   const PathWithLaneId & path, const DrivableAreaExpansionParameters & params);
 }  // namespace drivable_area_expansion
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_

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

@@ -28,7 +28,7 @@ namespace drivable_area_expansion
 /// @param[in] lanelet_map lanelet map
 /// @param[in] uncrossable_types types that cannot be crossed
 /// @return the uncrossable linestrings
-multilinestring_t extractUncrossableLines(
+multi_linestring_t extractUncrossableLines(
   const lanelet::LaneletMap & lanelet_map, const std::vector<std::string> & uncrossable_types);
 
 /// @brief Determine if the given linestring has one of the given types

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

@@ -30,13 +30,13 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Point;
 
 using point_t = tier4_autoware_utils::Point2d;
-using multipoint_t = tier4_autoware_utils::MultiPoint2d;
+using multi_point_t = tier4_autoware_utils::MultiPoint2d;
 using polygon_t = tier4_autoware_utils::Polygon2d;
 using ring_t = tier4_autoware_utils::LinearRing2d;
-using multipolygon_t = tier4_autoware_utils::MultiPolygon2d;
+using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d;
 using segment_t = tier4_autoware_utils::Segment2d;
 using linestring_t = tier4_autoware_utils::LineString2d;
-using multilinestring_t = tier4_autoware_utils::MultiLineString2d;
+using multi_linestring_t = tier4_autoware_utils::MultiLineString2d;
 
 struct PointDistance
 {

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

@@ -19,6 +19,9 @@
 #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
+#include "interpolation/linear_interpolation.hpp"
+
+#include <Eigen/Geometry>
 
 #include <boost/geometry.hpp>
 
@@ -32,7 +35,7 @@ void expandDrivableArea(
 {
   const auto uncrossable_lines =
     extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types);
-  multilinestring_t uncrossable_lines_in_range;
+  multi_linestring_t uncrossable_lines_in_range;
   const auto & p = path.points.front().point.pose.position;
   for (const auto & line : uncrossable_lines)
     if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length)
@@ -60,7 +63,7 @@ Point convert_point(const point_t & p)
 }
 
 polygon_t createExpandedDrivableAreaPolygon(
-  const PathWithLaneId & path, const multipolygon_t & expansion_polygons)
+  const PathWithLaneId & path, const multi_polygon_t & expansion_polygons)
 {
   polygon_t original_da_poly;
   original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1);
@@ -69,7 +72,7 @@ polygon_t createExpandedDrivableAreaPolygon(
     original_da_poly.outer().push_back(convert_point(*it));
   original_da_poly.outer().push_back(original_da_poly.outer().front());
 
-  multipolygon_t unions;
+  multi_polygon_t unions;
   auto expanded_da_poly = original_da_poly;
   for (const auto & p : expansion_polygons) {
     unions.clear();
@@ -82,85 +85,164 @@ polygon_t createExpandedDrivableAreaPolygon(
   return expanded_da_poly;
 }
 
-std::array<ring_t::const_iterator, 4> findLeftRightRanges(
-  const PathWithLaneId & path, const ring_t & expanded_drivable_area)
+void copy_z_over_arc_length(
+  const std::vector<geometry_msgs::msg::Point> & from, std::vector<geometry_msgs::msg::Point> & to)
+{
+  if (from.empty() || to.empty()) return;
+  to.front().z = from.front().z;
+  if (from.size() < 2 || to.size() < 2) return;
+  to.back().z = from.back().z;
+  auto i_from = 1lu;
+  auto s_from = tier4_autoware_utils::calcDistance2d(from[0], from[1]);
+  auto s_to = 0.0;
+  auto s_from_prev = 0.0;
+  for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) {
+    s_to += tier4_autoware_utils::calcDistance2d(to[i_to - 1], to[i_to]);
+    for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) {
+      s_from_prev = s_from;
+      s_from += tier4_autoware_utils::calcDistance2d(from[i_from], from[i_from + 1]);
+    }
+    if (s_from - s_from_prev != 0.0) {
+      const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev);
+      to[i_to].z = interpolation::lerp(from[i_from - 1].z, from[i_from].z, ratio);
+    } else {
+      to[i_to].z = to[i_to - 1].z;
+    }
+  }
+}
+
+void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area)
 {
+  const auto original_left_bound = path.left_bound;
+  const auto original_right_bound = path.right_bound;
   const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) {
     return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0;
   };
-  const auto is_left_of_path_start = [&](const point_t & p) {
-    return is_left_of_segment(
-      convert_point(path.points[0].point.pose.position),
-      convert_point(path.points[1].point.pose.position), p);
+  // prepare delimiting lines: start and end of the original expanded drivable area
+  const auto start_segment =
+    segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())};
+  const auto end_segment =
+    segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())};
+  point_t start_segment_center;
+  boost::geometry::centroid(start_segment, start_segment_center);
+  const auto path_start_segment =
+    segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)};
+  point_t end_segment_center;
+  boost::geometry::centroid(end_segment, end_segment_center);
+  const auto path_end_segment =
+    segment_t{convert_point(path.points.back().point.pose.position), end_segment_center};
+  const auto segment_to_line_intersection =
+    [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional<point_t> {
+    const auto line = Eigen::Hyperplane<double, 2>::Through(q1, q2);
+    const auto segment = Eigen::Hyperplane<double, 2>::Through(p1, p2);
+    const auto intersection = line.intersection(segment);
+    std::optional<point_t> result;
+    const auto is_on_segment =
+      (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x()
+                        : intersection.x() <= p1.x() && intersection.x() >= p2.x()) &&
+      (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y()
+                        : intersection.y() <= p1.y() && intersection.y() >= p2.y());
+    if (is_on_segment) result = point_t{intersection.x(), intersection.y()};
+    return result;
   };
-  const auto is_left_of_path_end = [&, size = path.points.size()](const point_t & p) {
-    return is_left_of_segment(
-      convert_point(path.points[size - 2].point.pose.position),
-      convert_point(path.points[size - 1].point.pose.position), p);
+  // find intersection between the expanded drivable area and the delimiting lines
+  const auto & da = expanded_drivable_area.outer();
+  struct Intersection
+  {
+    point_t intersection_point;
+    ring_t::const_iterator segment_it;
+    double distance = std::numeric_limits<double>::max();
+    explicit Intersection(ring_t::const_iterator it) : segment_it(it) {}
+    void update(const point_t & p, const ring_t::const_iterator & it, const double dist)
+    {
+      intersection_point = p;
+      segment_it = it;
+      distance = dist;
+    }
   };
-  const auto dist_to_path_start = [start = convert_point(path.points.front().point.pose.position)](
-                                    const auto & p) { return boost::geometry::distance(start, p); };
-  const auto dist_to_path_end = [end = convert_point(path.points.back().point.pose.position)](
-                                  const auto & p) { return boost::geometry::distance(end, p); };
-
-  double min_start_dist = std::numeric_limits<double>::max();
-  auto start_transition = expanded_drivable_area.end();
-  double min_end_dist = std::numeric_limits<double>::max();
-  auto end_transition = expanded_drivable_area.end();
-  for (auto it = expanded_drivable_area.begin(); std::next(it) != expanded_drivable_area.end();
-       ++it) {
-    if (is_left_of_path_start(*it) != is_left_of_path_start(*std::next(it))) {
-      const auto dist = dist_to_path_start(*it);
-      if (dist < min_start_dist) {
-        start_transition = it;
-        min_start_dist = dist;
-      }
+  Intersection start_left(da.end());
+  Intersection end_left(da.end());
+  Intersection start_right(da.end());
+  Intersection end_right(da.end());
+  for (auto it = da.begin(); it != da.end(); ++it) {
+    if (boost::geometry::distance(*it, start_segment.first) < 1e-3)
+      start_left.update(*it, it, 0.0);
+    else if (boost::geometry::distance(*it, start_segment.second) < 1e-3)
+      start_right.update(*it, it, 0.0);
+    else if (boost::geometry::distance(*it, end_segment.first) < 1e-3)
+      end_left.update(*it, it, 0.0);
+    else if (boost::geometry::distance(*it, end_segment.second) < 1e-3)
+      end_right.update(*it, it, 0.0);
+    const auto inter_start =
+      std::next(it) == da.end()
+        ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second)
+        : segment_to_line_intersection(
+            *it, *std::next(it), start_segment.first, start_segment.second);
+    if (inter_start) {
+      const auto dist = boost::geometry::distance(*inter_start, path_start_segment);
+      const auto is_left_of_path_start = is_left_of_segment(
+        convert_point(path.points[0].point.pose.position),
+        convert_point(path.points[1].point.pose.position), *inter_start);
+      if (is_left_of_path_start && dist < start_left.distance)
+        start_left.update(*inter_start, it, dist);
+      else if (!is_left_of_path_start && dist < start_right.distance)
+        start_right.update(*inter_start, it, dist);
     }
-    if (is_left_of_path_end(*it) != is_left_of_path_end(*std::next(it))) {
-      const auto dist = dist_to_path_end(*it);
-      if (dist < min_end_dist) {
-        end_transition = it;
-        min_end_dist = dist;
-      }
+    const auto inter_end =
+      std::next(it) == da.end()
+        ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second)
+        : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second);
+    if (inter_end) {
+      const auto dist = boost::geometry::distance(*inter_end, path_end_segment);
+      const auto is_left_of_path_end = is_left_of_segment(
+        convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end);
+      if (is_left_of_path_end && dist < end_left.distance)
+        end_left.update(*inter_end, it, dist);
+      else if (!is_left_of_path_end && dist < end_right.distance)
+        end_right.update(*inter_end, it, dist);
     }
   }
-  const auto left_start =
-    is_left_of_path_start(*start_transition) ? start_transition : std::next(start_transition);
-  const auto right_start =
-    is_left_of_path_start(*start_transition) ? std::next(start_transition) : start_transition;
-  const auto left_end =
-    is_left_of_path_end(*end_transition) ? end_transition : std::next(end_transition);
-  const auto right_end =
-    is_left_of_path_end(*end_transition) ? std::next(end_transition) : end_transition;
-  return {left_start, left_end, right_start, right_end};
-}
-
-void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area)
-{
+  if (  // ill-formed expanded drivable area -> keep the original bounds
+    start_left.segment_it == da.end() || start_right.segment_it == da.end() ||
+    end_left.segment_it == da.end() || end_right.segment_it == da.end()) {
+    return;
+  }
+  // extract the expanded left and right bound from the expanded drivable area
   path.left_bound.clear();
   path.right_bound.clear();
-  const auto begin = expanded_drivable_area.outer().begin();
-  const auto end = std::prev(expanded_drivable_area.outer().end());
-  const auto & [left_start, left_end, right_start, right_end] =
-    findLeftRightRanges(path, expanded_drivable_area.outer());
-  // NOTE: clockwise ordering -> positive increment for left bound, negative for right bound
-  if (left_start < left_end) {
-    path.left_bound.reserve(std::distance(left_start, left_end));
-    for (auto it = left_start; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it));
-  } else {  // loop back
-    path.left_bound.reserve(std::distance(left_start, end) + std::distance(begin, left_end));
-    for (auto it = left_start; it != end; ++it) path.left_bound.push_back(convert_point(*it));
-    for (auto it = begin; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it));
+  path.left_bound.push_back(convert_point(start_left.intersection_point));
+  path.right_bound.push_back(convert_point(start_right.intersection_point));
+  if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it))
+    path.right_bound.push_back(convert_point(*start_right.segment_it));
+  if (start_left.segment_it < end_left.segment_it) {
+    for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it)
+      path.left_bound.push_back(convert_point(*it));
+  } else {
+    for (auto it = std::next(start_left.segment_it); it < da.end(); ++it)
+      path.left_bound.push_back(convert_point(*it));
+    for (auto it = da.begin(); it <= end_left.segment_it; ++it)
+      path.left_bound.push_back(convert_point(*it));
   }
-  if (right_start > right_end) {
-    path.right_bound.reserve(std::distance(right_end, right_start));
-    for (auto it = right_start; it >= right_end; --it)
+  if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it))
+    path.left_bound.push_back(convert_point(end_left.intersection_point));
+  if (start_right.segment_it < end_right.segment_it) {
+    for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it)
+      path.right_bound.push_back(convert_point(*it));
+    for (auto it = std::prev(da.end()); it > end_right.segment_it; --it)
+      path.right_bound.push_back(convert_point(*it));
+  } else {
+    for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it)
       path.right_bound.push_back(convert_point(*it));
-  } else {  // loop back
-    path.right_bound.reserve(std::distance(begin, right_start) + std::distance(right_end, end));
-    for (auto it = right_start; it >= begin; --it) path.right_bound.push_back(convert_point(*it));
-    for (auto it = end - 1; it >= right_end; --it) path.right_bound.push_back(convert_point(*it));
   }
+  if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it)))
+    path.right_bound.push_back(convert_point(end_right.intersection_point));
+  // remove possible duplicated points
+  const auto point_cmp = [](const auto & p1, const auto & p2) {
+    return p1.x == p2.x && p1.y == p2.y;
+  };
+  std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp);
+  std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp);
+  copy_z_over_arc_length(original_left_bound, path.left_bound);
+  copy_z_over_arc_length(original_right_bound, path.right_bound);
 }
-
 }  // namespace drivable_area_expansion