perf(behavior_path_planner): simplify calculateDistanceLimit for dyna…

…mic drivable area expansion (autowarefoundation#4163) (#650)

* Add a test for the calculateDistanceLimit function



* Remove use of boost::geometry::within to improve performance



* [TMP] run behavior_path_planner with gdb



* Revert "[TMP] run behavior_path_planner with gdb"

This reverts commit b291af4.

---------

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

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

@@ -32,13 +32,19 @@ void expandDrivableArea(
 {
   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);
 }

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

@@ -25,10 +25,6 @@ double calculateDistanceLimit(
   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)
@@ -42,16 +38,10 @@ double calculateDistanceLimit(
 {
   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;
 }

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);
+  }
+}