fix(utils): add guard

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Oct 12, 2023 · add619c · add619c
1 parent ee1574a
commit add619c
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -1731,7 +1731,7 @@ std::vector<lanelet::ConstPoint3d> getBoundWithIntersectionAreas(
     const auto shared_point_itr_last =
       std::find_if(expanded_bound.rbegin(), expanded_bound.rend(), [&](const auto & p) {
         return std::any_of(
-          intersection_bound.begin(), intersection_bound.end(),
+          intersection_bound.rbegin(), intersection_bound.rend(),
           [&](const auto & point) { return point.id() == p.id(); });
       });
 
@@ -1757,6 +1757,13 @@ std::vector<lanelet::ConstPoint3d> getBoundWithIntersectionAreas(
         continue;
       }
 
+      // TODO(Satoshi OTA): remove this guard.
+      if (
+        std::distance(intersection_bound.begin(), trim_point_itr_last) <
+        std::distance(intersection_bound.begin(), trim_point_itr_init)) {
+        continue;
+      }
+
       std::vector<lanelet::ConstPoint3d> tmp_bound{};
 
       tmp_bound.insert(tmp_bound.end(), expanded_bound.begin(), shared_point_itr_init);