From fe4cb26d5f85d613836f44ee8de76e8f836298b5 Mon Sep 17 00:00:00 2001 From: yutaka Date: Wed, 14 Dec 2022 13:39:18 +0900 Subject: [PATCH 1/2] feat(behavior_path_planner): fix find nearest function from lateral distance --- planning/behavior_path_planner/src/utilities.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 056632edf0d31..0904f7e7589d1 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -80,7 +80,8 @@ size_t findNearestSegmentIndexFromLateralDistance( { size_t closest_idx = motion_utils::findNearestSegmentIndex(points, pose.position); double min_lateral_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, closest_idx, pose.position); + std::fabs(motion_utils::calcLateralOffset(points, pose.position, closest_idx)); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); @@ -94,6 +95,7 @@ size_t findNearestSegmentIndexFromLateralDistance( std::fabs(motion_utils::calcLateralOffset(points, pose.position, seg_idx)); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; + min_lateral_dist = lat_dist; } } From 790f5ecf7d5f41c129c7b825536221e688b325c3 Mon Sep 17 00:00:00 2001 From: yutaka Date: Wed, 14 Dec 2022 13:44:00 +0900 Subject: [PATCH 2/2] empty commit