fix(lanelet2_extension): fix getAngleDifference

map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp

@@ -21,6 +21,8 @@
 #include "lanelet2_extension/regulatory_elements/detection_area.hpp"
 #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
 
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
@@ -45,6 +47,11 @@ namespace lanelet
 {
 namespace utils
 {
+inline double getAngleDifference(const double angle1, const double angle2)
+{
+  const double diff_angle = std::abs(tier4_autoware_utils::normalizeRadian(angle1 - angle2));
+  return diff_angle;
+}
 namespace query
 {
 /**

map/lanelet2_extension/lib/query.cpp

@@ -20,7 +20,6 @@
 #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>
@@ -34,17 +33,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
 {

map/lanelet2_extension/test/src/test_query.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "lanelet2_extension/utility/query.hpp"
+#include "tier4_autoware_utils/math/normalization.hpp"
 
 #include <gtest/gtest.h>
 #include <math.h>
@@ -24,6 +25,50 @@ using lanelet::Point3d;
 using lanelet::Points3d;
 using lanelet::utils::getId;
 
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+TEST(normalizedAngleDifference, getAngleDifference)
+{
+  using lanelet::utils::getAngleDifference;
+
+  constexpr double eps = 0.1;
+  constexpr double pi = M_PI;
+  constexpr double pi2 = 2.0 * M_PI;
+  // case normal
+  {
+    // abs diff
+    EXPECT_FLOAT_EQ(getAngleDifference(-pi, 0), pi);
+    // abs diff
+    EXPECT_FLOAT_EQ(getAngleDifference(pi, 0), pi);
+  }
+  // case inverse angle
+  {
+    // return normalized angle diff
+    EXPECT_FLOAT_EQ(getAngleDifference(-pi - eps, pi), eps);
+    // return 0 if same direction
+    EXPECT_FLOAT_EQ(getAngleDifference(-pi, pi), 0);
+  }
+  // case more than 2PI angle
+  {
+    // return normalized angle diff
+    EXPECT_FLOAT_EQ(getAngleDifference(-pi2 - eps, pi2), eps);
+    // return pi if invert direction
+    EXPECT_FLOAT_EQ(getAngleDifference(-pi2, pi), pi);
+  }
+}
+
 class TestSuite : public ::testing::Test
 {
 public: