From 5ce9b563a7d303adb150cb5d50e9bcf7fe023921 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 13 Dec 2021 13:33:47 +0900 Subject: [PATCH] fix: fix angle diff calculation of mission_planner (#772) (#152) Signed-off-by: wep21 Co-authored-by: Takayuki Murooka Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> --- map/lanelet2_extension/lib/query.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/map/lanelet2_extension/lib/query.cpp b/map/lanelet2_extension/lib/query.cpp index 9cca2eb74b63a..1e9e9ae7c6c98 100644 --- a/map/lanelet2_extension/lib/query.cpp +++ b/map/lanelet2_extension/lib/query.cpp @@ -731,10 +731,11 @@ bool query::getClosestLanelet( if (angle_diff < min_angle) { min_angle = angle_diff; *closest_lanelet_ptr = llt; - } else if ((segment_angle - pose_yaw) < 1e-04) { - min_angle = std::abs(segment_angle - pose_yaw); - *closest_lanelet_ptr = llt; } + /* else if ((segment_angle - pose_yaw) < 1e-04) { + min_angle = std::abs(segment_angle - pose_yaw); + *closest_lanelet_ptr = llt; + }*/ } }