From 62a493a892e6a0936cc584a434ac694f9bbf0f5a Mon Sep 17 00:00:00 2001 From: hoangson02 Date: Wed, 15 Apr 2026 07:30:04 +0000 Subject: [PATCH] update --- src/custom_planner.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/custom_planner.cpp b/src/custom_planner.cpp index 8dc515d..eef6806 100755 --- a/src/custom_planner.cpp +++ b/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)); }