Merge pull request #1008 from tier4/hotfix/pr5516

fix(intersection): fix wrong resample process (autowarefoundation#5516)
tier4 · Nov 8, 2023 · e791528 · e791528
2 parents 2de2c91 + 8eaa02e
commit e791528
Showing 1 changed file with 5 additions and 5 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -1459,14 +1459,14 @@ lanelet::ConstLanelet generatePathLanelet(
 {
   lanelet::Points3d lefts;
   lanelet::Points3d rights;
+  size_t prev_idx = start_idx;
   for (size_t i = start_idx; i <= end_idx; ++i) {
     const auto & p = path.points.at(i).point.pose;
-    if (start_idx < i && i != end_idx) {
-      const auto & p_prev = path.points.at(i - 1).point.pose;
-      if (tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) {
-        continue;
-      }
+    const auto & p_prev = path.points.at(prev_idx).point.pose;
+    if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) {
+      continue;
     }
+    prev_idx = i;
     const double yaw = tf2::getYaw(p.orientation);
     const double x = p.position.x;
     const double y = p.position.y;