From 151a10b1e222295ee6438bbe58ebcee82f1a35d8 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 7 Jul 2023 14:02:31 +0900 Subject: [PATCH] perf(behavior_path_planner): simplify calculateDistanceLimit for dynamic drivable area expansion (#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 b291af4cb55ff2d83148f84bb9272bc61e1f86a8. --------- Signed-off-by: Maxime CLEMENT Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../drivable_area_expansion.cpp | 8 +++- .../drivable_area_expansion/expansion.cpp | 12 +----- .../test/test_drivable_area_expansion.cpp | 38 +++++++++++++++++++ 3 files changed, 46 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index a27010e4f8d15..4101ac6ab8c98 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/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); } diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 4bdbdedf44c77..79e15bf4e68d7 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/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::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::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; } diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index d1e9873e92c4c..7c4967be14bb8 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/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::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); + } +}