From 7c84705ce737736d53829916eb36c9860c10c418 Mon Sep 17 00:00:00 2001
From: hoangson02 <hiepleminh01@gmail.com>
Date: Mon, 23 Mar 2026 03:02:29 +0000
Subject: [PATCH] update calcDirecPath

---
 src/custom_planner.cpp | 24 +++++++++++++++---------
 1 file changed, 15 insertions(+), 9 deletions(-)

diff --git a/src/custom_planner.cpp b/src/custom_planner.cpp
index a706b00..3db0155 100755
--- a/src/custom_planner.cpp
+++ b/src/custom_planner.cpp
@@ -793,7 +793,8 @@ namespace custom_planner
     if(!rotating_robot_plag_)
     {
       if(total_two_curve > 0)
-      {        
+      {       
+        robot::log_warning("DEBUG 1"); 
         if(total_two_curve == 1)
         {
           vector<Pose> Pose_start_tmp;
@@ -883,14 +884,19 @@ namespace custom_planner
       }
       else
       {
+        robot::log_warning("DEBUG 2"); 
         if(merge_path_calc_.status_yaw_edge_end_)
         {
+          robot::log_warning("DEBUG 2.1"); 
           reverse_ = true;
+          
           // printf("custom_planner][calcAllYaw] DEBUG 300");
           setYawAllPosesOnEdge(savePosesOnEdge,true);
+          
         }
         else
         {
+          robot::log_warning("DEBUG 2.2"); 
           reverse_ = false;
           // printf("custom_planner][calcAllYaw] DEBUG 310");
           setYawAllPosesOnEdge(savePosesOnEdge,false);
@@ -1151,16 +1157,16 @@ namespace custom_planner
 
     double yaw_goal = data_convert::getYaw(goal.pose.orientation);
     // printf("[custom_planner] DEBUG: angle: %f; yaw_goal: %f",angle, yaw_goal);
-    auto normalizeAngle0To2Pi = [](double angle) -> double
-    {
-        angle = fmod(angle, 2.0 * M_PI);
-        if (angle < 0)
-            angle += 2.0 * M_PI;
-        return angle;
+    // 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;
+      return a;
     };
 
-    double delta_angle = fabs((normalizeAngle0To2Pi(yaw_goal)) - (normalizeAngle0To2Pi(angle)));
-    return (delta_angle > (0.5*M_PI));
+    double delta_angle = std::fabs(std::fabs(normalizeAngle(yaw_goal)) - std::fabs(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));
   }
 
 // Export factory function