From e0745ed1863af0662568034e8d5d8206110d79ac Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 18 Dec 2023 09:13:47 +0900 Subject: [PATCH 1/2] Use a lambda and for_each to potentially reduce computation time Signed-off-by: Daniel Sanchez --- .../static_drivable_area.cpp | 42 ++++++++++++------- 1 file changed, 26 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index f6344b94a6bc5..35e5ca4c517f4 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -586,24 +586,34 @@ std::vector cutOverlappedLanes( [&has_same_id_lane, &p](const auto & lanelet) { return has_same_id_lane(lanelet, p); }); }; - // Step1. find first path point within drivable lanes - size_t start_point_idx = std::numeric_limits::max(); - for (const auto & lanes : shorten_lanes) { - for (size_t i = 0; i < original_points.size(); ++i) { - // check right lane - if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + const auto is_point_in_drivable_lanes = [&has_same_id_lane, &has_same_id_lanes, &original_points]( + const auto & lanes, size_t i) { + if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { + return true; + } + // check left lane + if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { + return true; + } + // check middle lanes + if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { + return true; + } + return false; + }; - // check left lane - if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + // Step1. find first path point within drivable lanes + size_t start_point_idx = original_points.size(); - // check middle lanes - if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + for (size_t i = 0; i < original_points.size(); ++i) { + const bool first_path_point_in_drivable_lane_found = std::any_of( + shorten_lanes.begin(), shorten_lanes.end(), + [&is_point_in_drivable_lanes, i](const auto & lanes) { + return is_point_in_drivable_lanes(lanes, i); + }); + if (first_path_point_in_drivable_lane_found) { + start_point_idx = i; + break; } } From 11f05dbf74f58ee57a1a684f1c790f06c211e328 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 19 Dec 2023 11:47:29 +0900 Subject: [PATCH 2/2] further simplify the code with lambda Signed-off-by: Daniel Sanchez --- .../static_drivable_area.cpp | 30 +++++-------------- 1 file changed, 8 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 35e5ca4c517f4..c6558a091d893 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -586,17 +586,17 @@ std::vector cutOverlappedLanes( [&has_same_id_lane, &p](const auto & lanelet) { return has_same_id_lane(lanelet, p); }); }; - const auto is_point_in_drivable_lanes = [&has_same_id_lane, &has_same_id_lanes, &original_points]( - const auto & lanes, size_t i) { - if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { + const auto is_point_in_drivable_lanes = [&has_same_id_lane, &has_same_id_lanes]( + const auto & lanes, const auto & p) { + if (has_same_id_lane(lanes.right_lane, p)) { return true; } // check left lane - if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { + if (has_same_id_lane(lanes.left_lane, p)) { return true; } // check middle lanes - if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { + if (has_same_id_lanes(lanes.middle_lanes, p)) { return true; } return false; @@ -608,8 +608,8 @@ std::vector cutOverlappedLanes( for (size_t i = 0; i < original_points.size(); ++i) { const bool first_path_point_in_drivable_lane_found = std::any_of( shorten_lanes.begin(), shorten_lanes.end(), - [&is_point_in_drivable_lanes, i](const auto & lanes) { - return is_point_in_drivable_lanes(lanes, i); + [&is_point_in_drivable_lanes, &original_points, i](const auto & lanes) { + return is_point_in_drivable_lanes(lanes, original_points.at(i)); }); if (first_path_point_in_drivable_lane_found) { start_point_idx = i; @@ -620,24 +620,10 @@ std::vector cutOverlappedLanes( // Step2. pick up only path points within drivable lanes for (const auto & lanes : shorten_lanes) { for (size_t i = start_point_idx; i < original_points.size(); ++i) { - // check right lane - if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { + if (is_point_in_drivable_lanes(lanes, original_points.at(i))) { path.points.push_back(original_points.at(i)); continue; } - - // check left lane - if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { - path.points.push_back(original_points.at(i)); - continue; - } - - // check middle lanes - if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { - path.points.push_back(original_points.at(i)); - continue; - } - start_point_idx = i; break; }