fix(static_drivable_area_expansion): fix bound extraction logic

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

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -148,6 +148,40 @@
 
   return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1);
 }
+
+std::vector<lanelet::ConstPoint3d> extractBoundFromPolygon(
+  const lanelet::ConstPolygon3d & polygon, const size_t start_idx, const size_t end_idx,
+  const bool clockwise)
+{
+  std::vector<lanelet::ConstPoint3d> ret{};
+  for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) {
+    ret.push_back(polygon[i]);
+
+    if (i + 1 == polygon.size() && clockwise) {
+      if (end_idx == 0) {
+        ret.push_back(polygon[end_idx]);
+        return ret;
+      }
+      i = 0;
+      ret.push_back(polygon[i]);
+      continue;
+    }
+
+    if (i == 0 && !clockwise) {
+      if (end_idx == polygon.size() - 1) {
+        ret.push_back(polygon[end_idx]);
+        return ret;
+      }
+      i = polygon.size() - 1;
+      ret.push_back(polygon[i]);
+      continue;
+    }
+  }
+
+  ret.push_back(polygon[end_idx]);
+
+  return ret;
+}
 }  // namespace
 
 namespace behavior_path_planner::utils::drivable_area_processing
@@ -1260,83 +1294,53 @@
   const std::vector<lanelet::ConstPoint3d> & original_bound,
   const std::shared_ptr<RouteHandler> & route_handler,
   const std::vector<DrivableLanes> & drivable_lanes, const bool is_left)
 {
-  const auto extract_bound_from_polygon =
-    [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) {
-      std::vector<lanelet::ConstPoint3d> ret{};
-      for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) {
-        ret.push_back(polygon[i]);
-
-        if (i + 1 == polygon.size() && clockwise) {
-          if (end_idx == 0) {
-            ret.push_back(polygon[end_idx]);
-            return ret;
-          }
-          i = 0;
-          continue;
-        }
-
-        if (i == 0 && !clockwise) {
-          if (end_idx == polygon.size() - 1) {
-            ret.push_back(polygon[end_idx]);
-            return ret;
-          }
-          i = polygon.size() - 1;
-          continue;
-        }
-      }
-
-      ret.push_back(polygon[end_idx]);
-
-      return ret;
-    };
-
   std::vector<lanelet::ConstPoint3d> expanded_bound = original_bound;
 
   // expand drivable area by using intersection area.
   for (const auto & drivable_lane : drivable_lanes) {
     const auto edge_lanelet = is_left ? drivable_lane.left_lane : drivable_lane.right_lane;
     const auto lanelet_bound = is_left ? edge_lanelet.leftBound3d() : edge_lanelet.rightBound3d();
 
     if (lanelet_bound.size() < 2) {
       continue;
     }
 
     const std::string id = edge_lanelet.attributeOr("intersection_area", "else");
     if (id == "else") {
       continue;
     }
 
     // Step1. extract intersection partial bound.
     std::vector<lanelet::ConstPoint3d> intersection_bound{};
     {
       const auto polygon =
         route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str()));
 
       const auto is_clockwise_polygon =
         boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon()));
       const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left;
 
       const auto intersection_bound_itr_init = std::find_if(
         polygon.begin(), polygon.end(),
         [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.front().id(); });
 
       const auto intersection_bound_itr_last = std::find_if(
         polygon.begin(), polygon.end(),
         [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.back().id(); });
 
       if (
         intersection_bound_itr_init == polygon.end() ||
         intersection_bound_itr_last == polygon.end()) {
         continue;
       }
 
       // extract line strings between start_idx and end_idx.
       const size_t start_idx = std::distance(polygon.begin(), intersection_bound_itr_init);
       const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last);
 
       intersection_bound =
-        extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration);
+        extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration);
     }
 
     // Step2. check shared bound point.