update v

src/custom_planner.cpp

@@ -51,6 +51,7 @@ namespace custom_planner
       plan.clear();
 
       // Check for reinitialization needs
+      robot::log_error("yaw goal: %f", data_convert::getYaw(goal.pose.orientation));
       bool do_init = false;
       if(!makePlanWithOrder(msg, start, goal)) return false;
       if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
@@ -794,6 +795,7 @@ namespace custom_planner
     {
       if(total_two_curve > 0)
       {       
+        robot::log_warning("DEBUG 1"); 
         if(total_two_curve == 1)
         {
           vector<Pose> Pose_start_tmp;
@@ -883,14 +885,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,15 +1158,24 @@ 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
+    // nomalize angle [-pi, pi] to [0, 2*pi]
+    auto normalizeAngle = [](double a) -> double
     { 
-        angle = fmod(angle, 2.0 * M_PI);
+      while (a > 2 * M_PI) a -= 2 * M_PI;
-        if (angle < 0)
+      while (a < 0) a += 2 * M_PI;
-            angle += 2.0 * M_PI;
+      return a;
-        return angle;
     };
 
-    double delta_angle = fabs((normalizeAngle0To2Pi(yaw_goal)) - (normalizeAngle0To2Pi(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));
   }
 

src/merge_path_calc.cpp

@@ -177,9 +177,11 @@ namespace custom_planner {
         // Các hằng số chung cho cả START và GOAL
         static const double MAX_DISTANCE = 1.0;  // Ngưỡng khoảng cách tối đa (m)
         static const double MIN_DISTANCE = 0.1;  // Khoảng cách tối thiểu (m)
-        static const double MIN_SCALE = 0.2;     // Giá trị tối thiểu của hệ số
+        static const double MIN_SCALE = 0.5;     // Giá trị tối thiểu của hệ số
         static const double MAX_SCALE = 1.0;     // Giá trị tối đa của hệ số
-        static const double MAX_ANGLE_THRESHOLD = 0.77 * M_PI; // Ngưỡng góc tối đa
+        static const double MAX_ANGLE_THRESHOLD = 0.9 * M_PI; // Ngưỡng góc tối đa
+        static const double TURNING_GAIN = 0.8;
+        static const double CALIB_VAL = 0.5;
         
         int nearest_idx = 0;
         
@@ -218,7 +220,7 @@ namespace custom_planner {
                         (MAX_DISTANCE - MIN_DISTANCE);
             
             // === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC ===
-            double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI;
+            double angle_threshold = (CALIB_VAL + std::abs(scale)) * TURNING_GAIN * M_PI;
             if(angle_threshold >= MAX_ANGLE_THRESHOLD) 
                 angle_threshold = MAX_ANGLE_THRESHOLD;