From 96e62013aac31e62ed2ed80b29da1879aa984633 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Thu, 9 Dec 2021 17:13:59 +0900
Subject: [PATCH] fix: fix angle diff calculation of mission_planner (#772)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>
---
 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;
+       }*/
     }
   }