Normalize angle in ros conversions

source/armarx/navigation/local_planning/ros_conversions.cpp

@@ -49,6 +49,8 @@ namespace armarx::navigation::conv
         // we change it such that x is pointing forward.
         // ROS: x pointing forward, ArmarX: y pointing forward
         theta += M_PI_2;
+        // normalize angle
+        theta = g2o::normalize_theta(theta);
 
         return {pos / 1000., theta}; // [mm] to [m]
     }