fix(lanelet2_extension): add guard for inner product (#256)

* fix: add guard for inner product Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * ci(pre-commit): autofix Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * fix: delete unnecessary comments Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * Update map/lanelet2_extension/lib/query.cpp Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * delete equal in comparison operator Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * fix: use normilized radian for calculation of angle difference Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
tier4 · Feb 17, 2022 · 633fa5d · 633fa5d
1 parent 37a2a6e
commit 633fa5d
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 9 deletions.
diff --git a/map/lanelet2_extension/lib/query.cpp b/map/lanelet2_extension/lib/query.cpp
@@ -20,6 +20,7 @@
 #include "lanelet2_extension/utility/utilities.hpp"
 
 #include <Eigen/Eigen>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <lanelet2_core/geometry/Lanelet.h>
 #include <lanelet2_routing/RoutingGraph.h>
@@ -37,11 +38,10 @@ namespace
 {
 double getAngleDifference(const double angle1, const double angle2)
 {
-  Eigen::Vector2d vec1, vec2;
-  vec1 << std::cos(angle1), std::sin(angle1);
-  vec2 << std::cos(angle2), std::sin(angle2);
-  const double diff_angle = std::acos(vec1.dot(vec2));
-  return std::fabs(diff_angle);
+  const double normalized_angle1 = tier4_autoware_utils::normalizeRadian(angle1);
+  const double normalized_angle2 = tier4_autoware_utils::normalizeRadian(angle2);
+  const double diff_angle = std::fabs(normalized_angle1 - normalized_angle2);
+  return diff_angle;
 }
 
 }  // namespace
@@ -732,10 +732,6 @@ bool query::getClosestLanelet(
         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;
-       }*/
     }
   }
 

diff --git a/map/lanelet2_extension/package.xml b/map/lanelet2_extension/package.xml
@@ -24,6 +24,7 @@
   <depend>pugixml-dev</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
+  <depend>tier4_autoware_utils</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>