fix(lanelet2_extension): fix getAngleDifference (autowarefoundation#264)

* chore(query): update and add gtest

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* ci(pre-commit): autofix

* chore(query): add deprecated attribute to getAngleDifference

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(query): deprecated as comment to avoid clang errror

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Revert "chore(query): deprecated as comment to avoid clang errror"

This reverts commit 28cc95d.

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Revert "chore(query): add deprecated attribute to getAngleDifference"

This reverts commit 3b56ce7.

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Revert "ci(pre-commit): autofix"

This reverts commit 61e05c4.

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* Revert "chore(query): update and add gtest"

This reverts commit 027c4cf.

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(lanelet2_extension): fix getAngleDifference

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(lanelet2_extension): ref value

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

Author: 3 people

map/lanelet2_extension/lib/query.cpp

@@ -34,17 +34,6 @@
 #include <vector>
 
 using lanelet::utils::to2D;
-namespace
-{
-double getAngleDifference(const double angle1, const double angle2)
-{
-  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
 
 namespace lanelet
 {
@@ -727,7 +716,7 @@ bool query::getClosestLanelet(
       lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline());
       double segment_angle = std::atan2(
         segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x());
-      double angle_diff = getAngleDifference(segment_angle, pose_yaw);
+      double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian(segment_angle - pose_yaw));
       if (angle_diff < min_angle) {
         min_angle = angle_diff;
         *closest_lanelet_ptr = llt;