From ea105577465f681b6081a176c027a1f7830ebdde Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 8 Nov 2023 14:28:19 +0900 Subject: [PATCH] fix(intersection): fix wrong resample process (#5516) Signed-off-by: Mamoru Sobue --- .../behavior_velocity_intersection_module/src/util.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index d983179d6c5c1..10db86f1e35c4 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1428,14 +1428,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;