From d2c2cdaf53d2ce23c6d194944987181e9cb7855e Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 13 Nov 2024 17:23:39 +0900 Subject: [PATCH] fix overlap removal function Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/utils.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 1dbaba066ce64..3cc2aecd2d06f 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -200,17 +200,18 @@ std::vector>> get_wa void remove_overlapping_points(PathWithLaneId & path) { - auto & filtered_path_end = path.points.front(); + auto filtered_path_end_it = path.points.begin(); for (auto it = std::next(path.points.begin()); it != path.points.end();) { constexpr auto min_interval = 0.001; if ( - autoware::universe_utils::calcDistance3d(filtered_path_end.point, it->point) < min_interval) { - filtered_path_end.lane_ids.push_back(it->lane_ids.front()); - filtered_path_end.point.longitudinal_velocity_mps = std::min( - it->point.longitudinal_velocity_mps, filtered_path_end.point.longitudinal_velocity_mps); + autoware::universe_utils::calcDistance3d(filtered_path_end_it->point, it->point) < + min_interval) { + filtered_path_end_it->lane_ids.push_back(it->lane_ids.front()); + filtered_path_end_it->point.longitudinal_velocity_mps = std::min( + filtered_path_end_it->point.longitudinal_velocity_mps, it->point.longitudinal_velocity_mps); it = path.points.erase(it); } else { - filtered_path_end = *it; + filtered_path_end_it = it; ++it; } }