Merge pull request #78 from AutoModality/AM-729/uam_all_headings

fix: added wrap_2pi

Author: danhennage

include/vb_util_lib/am_geometry.h

@@ -50,6 +50,9 @@ struct PointStamp {
 // This takes any angle and makes sure that it is between -PI and +PI
 double wrap_pi(double bearing);
 
+// This takes any angle and makes sure that it is between 0 and 2 * PI
+double wrap_2pi(double bearing);
+
 //==================================
 // Distances and arc lengths
 //==================================

src/vb_util_lib/am_geometry.cpp

@@ -26,6 +26,21 @@ double wrap_pi(double bearing)
   return bearing;
 }
 
+double wrap_2pi(double bearing)
+{
+  while (bearing >= 2 * _PI_R)
+    {
+      bearing -= _TWOPI_R;
+    }
+
+  while (bearing <= 0)
+    {
+      bearing += _TWOPI_R;
+    }
+
+  return bearing;
+}
+
 double pointDistance(geometry_msgs::Point& p1, geometry_msgs::Point& p2) {
     double xd = p2.x - p1.x;
     double yd = p2.y - p1.y;