fix(utils): fix invalid insertion

Signed-off-by: satoshi-ota <[email protected]>

planning/behavior_path_planner/src/utils/utils.cpp

@@ -1722,7 +1722,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(); });
       });
 
@@ -1748,6 +1748,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);