feat(behavior_path_planner): potentially speed up cut overlapped lanes (

autowarefoundation#5906)

* Use a lambda and for_each to potentially reduce computation time

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* further simplify the code with lambda

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

---------

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -586,48 +586,44 @@ std::vector<DrivableLanes> 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<size_t>::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](
+                                            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, p)) {
+      return true;
+    }
+    // check middle lanes
+    if (has_same_id_lanes(lanes.middle_lanes, p)) {
+      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, &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;
+      break;
     }
   }
 
   // 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))) {
-        path.points.push_back(original_points.at(i));
-        continue;
-      }
-
-      // check left lane
-      if (has_same_id_lane(lanes.left_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 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;
     }