From 5375a5ea844cd9f3424f864d34e1184373a1c9f4 Mon Sep 17 00:00:00 2001
From: QUYVN <quyvn@phenikaa-x.com>
Date: Tue, 24 Mar 2026 08:05:05 +0000
Subject: [PATCH] update

---
 .../mkt_algorithm/src/diff/diff_predictive_trajectory.cpp   | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
index 5a12570..1843f90 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
@@ -477,6 +477,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
   drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
+
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
@@ -512,7 +513,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
   allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
-
+  allow_rotate &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1;
   double angle_to_heading;
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
   {
@@ -548,7 +549,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       robot_nav_2d_msgs::Twist2D drive_target;
       transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
       carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
-
       // Normal Pure Pursuit
       this->computePurePursuit(
       carrot_pose, 
@@ -585,6 +585,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     break;
   }
 
+  if(fabs(v_max == 0.0))
+    drive_cmd.x = 0.0;
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;