update

src/custom_planner.cpp

@@ -1157,14 +1157,23 @@ namespace custom_planner
 
     double yaw_goal = data_convert::getYaw(goal.pose.orientation);
     // printf("[custom_planner] DEBUG: angle: %f; yaw_goal: %f",angle, yaw_goal);
-    // nomalize angle to [-pi, pi]
-    auto normalizeAngle = [](double a) {
-      while (a > M_PI) a -= 2 * M_PI;
-      while (a < -M_PI) a += 2 * M_PI;
+    // nomalize angle [-pi, pi] to [0, 2*pi]
+    auto normalizeAngle = [](double a) -> double
+    { 
+      while (a > 2 * M_PI) a -= 2 * M_PI;
+      while (a < 0) a += 2 * M_PI;
       return a;
     };
 
-    double delta_angle = std::fabs(normalizeAngle(yaw_goal) - normalizeAngle(angle));
+    auto wrapToPi = [](double a) -> double
+    {
+        while (a > M_PI) a -= 2.0 * M_PI;
+        while (a < -M_PI) a += 2.0 * M_PI;
+        return a;
+    };
+
+    double delta_angle = std::fabs(wrapToPi(normalizeAngle(yaw_goal) - normalizeAngle(angle)));
+
     robot::log_warning("DEBUG 3: yaw_goal: %f, angle: %f, delta_angle: %f", yaw_goal, angle, delta_angle);
     return (delta_angle > (0.5 * M_PI));
   }