perf(behavior_path_planner): simplify calculateDistanceLimit for dynamic drivable area expansion (#4163)

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

@@ -32,13 +32,19 @@
 {
   const auto uncrossable_lines =
     extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types);
+  multilinestring_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)
+      uncrossable_lines_in_range.push_back(line);
   const auto path_footprints = createPathFootprints(path, params);
   const auto predicted_paths = createObjectFootprints(dynamic_objects, params);
   const auto expansion_polygons =
     params.expansion_method == "lanelet"
       ? createExpansionLaneletPolygons(
           path_lanes, route_handler, path_footprints, predicted_paths, params)
-      : createExpansionPolygons(path, path_footprints, predicted_paths, uncrossable_lines, params);
+      : createExpansionPolygons(
+          path, path_footprints, predicted_paths, uncrossable_lines_in_range, params);
   const auto expanded_drivable_area = createExpandedDrivableAreaPolygon(path, expansion_polygons);
   updateDrivableAreaBounds(path, expanded_drivable_area);
 }
@@ -54,7 +60,7 @@
 }
 
 polygon_t createExpandedDrivableAreaPolygon(
   const PathWithLaneId & path, const multipolygon_t & expansion_polygons)
 {
   polygon_t original_da_poly;
   original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1);
@@ -63,7 +69,7 @@
     original_da_poly.outer().push_back(convert_point(*it));
   original_da_poly.outer().push_back(original_da_poly.outer().front());
 
   multipolygon_t unions;
   auto expanded_da_poly = original_da_poly;
   for (const auto & p : expansion_polygons) {
     unions.clear();

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

@@ -22,13 +22,9 @@
 
 double calculateDistanceLimit(
   const linestring_t & base_ls, const polygon_t & expansion_polygon,
   const multilinestring_t & limit_lines)
 {
   auto dist_limit = std::numeric_limits<double>::max();
-  boost::geometry::for_each_point(limit_lines, [&](const auto & point) {
-    if (boost::geometry::within(point, expansion_polygon))
-      dist_limit = std::min(dist_limit, boost::geometry::distance(point, base_ls));
-  });
   multipoint_t intersections;
   boost::geometry::intersection(expansion_polygon, limit_lines, intersections);
   for (const auto & p : intersections)
@@ -38,20 +34,14 @@
 
 double calculateDistanceLimit(
   const linestring_t & base_ls, const polygon_t & expansion_polygon,
   const multipolygon_t & limit_polygons)
 {
   auto dist_limit = std::numeric_limits<double>::max();
   for (const auto & polygon : limit_polygons) {
-    for (const auto & p : polygon.outer()) {
-      if (boost::geometry::within(p, expansion_polygon)) {
-        dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls));
-      }
-    }
     multipoint_t intersections;
     boost::geometry::intersection(expansion_polygon, polygon, intersections);
-    for (const auto & p : intersections) {
+    for (const auto & p : intersections)
       dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls));
-    }
   }
   return dist_limit;
 }
@@ -60,7 +50,7 @@
   const linestring_t & base_ls, const double dist, const bool is_left_side)
 {
   namespace strategy = boost::geometry::strategy::buffer;
   multipolygon_t polygons;
   // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer
   constexpr auto zero = 0.1;
   const auto left_dist = is_left_side ? dist : zero;
@@ -103,7 +93,7 @@
 
 polygon_t create_compensation_polygon(
   const linestring_t & base_ls, const double compensation_dist, const bool is_left,
   const multilinestring_t uncrossable_lines, const multipolygon_t & predicted_paths)
 {
   polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left);
   double dist_limit = std::min(
@@ -116,8 +106,8 @@
   return compensation_polygon;
 }
 
 multipolygon_t createExpansionPolygons(
   const PathWithLaneId & path, const multipolygon_t & path_footprints,
   const multipolygon_t & predicted_paths, const multilinestring_t & uncrossable_lines,
   const DrivableAreaExpansionParameters & params)
 {

planning/behavior_path_planner/test/test_drivable_area_expansion.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 #include "lanelet2_extension/utility/message_conversion.hpp"
@@ -288,3 +289,40 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea)
   EXPECT_NEAR(path.right_bound[2].x, 2.0, eps);
   EXPECT_NEAR(path.right_bound[2].y, -1.0, eps);
 }
+
+TEST(DrivableAreaExpansion, calculateDistanceLimit)
+{
+  using drivable_area_expansion::calculateDistanceLimit;
+  using drivable_area_expansion::linestring_t;
+  using drivable_area_expansion::multilinestring_t;
+  using drivable_area_expansion::polygon_t;
+
+  {
+    const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}};
+    const multilinestring_t uncrossable_lines = {};
+    const polygon_t expansion_polygon = {
+      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}};
+    const auto limit_distance =
+      calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines);
+    EXPECT_NEAR(limit_distance, std::numeric_limits<double>::max(), 1e-9);
+  }
+  {
+    const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}};
+    const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}};
+    const polygon_t expansion_polygon = {
+      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}};
+    const auto limit_distance =
+      calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line});
+    EXPECT_NEAR(limit_distance, 2.0, 1e-9);
+  }
+  {
+    const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}};
+    const multilinestring_t uncrossable_lines = {
+      {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}};
+    const polygon_t expansion_polygon = {
+      {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}};
+    const auto limit_distance =
+      calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines);
+    EXPECT_NEAR(limit_distance, 1.0, 1e-9);
+  }
+}