Using angles library to wrap angles

cra-ros-pkg · Oct 24, 2022 · 710d68e · 710d68e
1 parent bf02bfb
commit 710d68e
Showing 1 changed file with 3 additions and 11 deletions.
diff --git a/src/filter_utilities.cpp b/src/filter_utilities.cpp
@@ -33,6 +33,8 @@
 #include "robot_localization/filter_utilities.h"
 #include "robot_localization/filter_common.h"
 
+#include <angles/angles.h>
+
 #include <string>
 #include <vector>
 
@@ -128,17 +130,7 @@ namespace FilterUtilities
 
   double clampRotation(double rotation)
   {
-    while (rotation > PI)
-    {
-      rotation -= TAU;
-    }
-
-    while (rotation < -PI)
-    {
-      rotation += TAU;
-    }
-
-    return rotation;
+    return angles::normalize_angle(rotation);
   }
 
 }  // namespace FilterUtilities