feat(lanelet2_extension)!: remove dependency on autoware_utils

autoware_lanelet2_extension/lib/query.cpp

@@ -28,9 +28,6 @@
 #include "autoware_lanelet2_extension/utility/message_conversion.hpp"
 #include "autoware_lanelet2_extension/utility/utilities.hpp"
 
-#include <Eigen/Eigen>
-#include <autoware_utils/autoware_utils.hpp>
-
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/Lanelet.h>
 #include <lanelet2_core/primitives/Lanelet.h>
@@ -54,6 +51,24 @@
 
 using lanelet::utils::to2D;
 
+namespace
+{
+
+inline double normalize_radian(const double rad)
+{
+  constexpr double pi = 3.14159265358979323846;  // To be replaced by std::numbers::pi in C++20
+  constexpr double min_rad = -pi;
+  const auto max_rad = min_rad + 2 * pi;
+
+  const auto value = std::fmod(rad, 2 * pi);
+  if (min_rad <= value && value < max_rad) {
+    return value;
+  }
+
+  return value - std::copysign(2 * pi, value);
+}
+}  // namespace
+
 namespace lanelet::utils
 {
 
@@ -934,7 +949,7 @@
       if (!segment.empty()) {
         double segment_angle = std::atan2(
           segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x());
-        angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw));
+        angle_diff = std::abs(::normalize_radian(segment_angle - pose_yaw));
       }
       if (angle_diff < min_angle) {
         min_angle = angle_diff;
@@ -996,7 +1011,7 @@
       const auto & distance = llt_pair.second;
 
       double lanelet_angle = getLaneletAngle(llt_pair.first, search_pose.position);
-      double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
+      double angle_diff = std::abs(::normalize_radian(lanelet_angle - pose_yaw));
 
       if (angle_diff > std::abs(yaw_threshold)) continue;
       if (min_distance < distance) break;

autoware_lanelet2_extension/package.xml

@@ -17,7 +17,6 @@
 
   <depend>autoware_map_msgs</depend>
   <depend>autoware_planning_msgs</depend>
-  <depend>autoware_utils</depend>
   <depend>geographiclib</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_core</depend>