Tạm bỏ

CATKIN_BUILD_README.md

@@ -20,7 +20,11 @@ The specified base path contains a CMakeLists.txt but "catkin_make" must be invo
 
 # Build trong workspace mới
 cd ../pnkx_nav_catkin_ws
-catkin_make
+rm -rf build devel
+catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo \
+  -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \
+  -DCMAKE_C_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \
+  -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=address"
 source devel/setup.bash
 ```
 

CMAKE_BUILD_README.md

@@ -356,8 +356,7 @@ export PNKX_NAV_CORE_CONFIG_DIR=/path/to/config
 
 # Chỉ định workspace directory
 export PNKX_NAV_CORE_DIR=/path/to/pnkx_nav_core
-
+ ] không install)
-# LD_LIBRARY_PATH (nếu không install)
 export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/pnkx_nav_core/build/lib
 ```
 

config/move_base_common_params.yaml

@@ -7,7 +7,7 @@ base_global_planner: CustomPlanner
 
 PNKXLocalPlanner:
   base_local_planner: LocalPlannerAdapter
-  base_global_planner: CustomPlanner
+  base_global_planner: CustomPlanner  
 
 PNKXDockingLocalPlanner:
   base_local_planner: LocalPlannerAdapter
@@ -30,7 +30,7 @@ max_planning_retries: 0     # ... or after 10 attempts (whichever happens first)
 oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
 oscillation_distance: 0.5
 ### recovery behaviors
-recovery_behavior_enabled: true
+recovery_behavior_enabled: false 
 recovery_behaviors: [       
                       {name: aggressive_reset, type: ClearCostmapRecovery},
                       {name: conservative_reset, type: ClearCostmapRecovery},                      

config/pnkx_local_planner_params.yaml

@@ -1,5 +1,5 @@
-yaw_goal_tolerance: 0.03  
+yaw_goal_tolerance: 0.02  
-xy_goal_tolerance: 0.02 
+xy_goal_tolerance: 0.03 
 min_approach_linear_velocity: 0.05 
 
 LocalPlannerAdapter:
@@ -53,12 +53,12 @@ LimitedAccelGenerator:
   max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
-  acc_lim_x: 1.5
+  acc_lim_x: 3.0
   acc_lim_y: 0.0      # diff drive robot
   acc_lim_theta: 1.5
-  decel_lim_x: -1.5
+  decel_lim_x: -3.0
   decel_lim_y: -0.0
-  decel_lim_theta: -1.5
+  decel_lim_theta: -2.0
 
   # Whether to split the path into segments or not
   split_path: true
@@ -74,8 +74,8 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
-  angle_threshold: 0.47
+  angle_threshold: 0.6
   index_samples: 60
   follow_step_path: true
 
@@ -119,7 +119,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.8
   index_samples: 60
   follow_step_path: true
@@ -156,7 +156,7 @@ MKTAlgorithmDiffGoStraight:
 
 MKTAlgorithmDiffRotateToGoal:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.47
   index_samples: 60
   follow_step_path: true

src/APIs/c_api/src/nav_c_api.cpp

@@ -569,11 +569,16 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
       return false;
 
     robot_geometry_msgs::Vector3 linear;
-    linear.x = linear_x;
+    linear.x = 0.1;
     linear.y = linear_y;
     linear.z = linear_z;
-    robot::log_info("setTwistLinear %f", linear.x);
+    bool result = nav_ptr->setTwistLinear(linear);
-    return nav_ptr->setTwistLinear(linear);
+    robot::log_info("setTwistLinear Forward %f", linear.x);
+    
+    linear.x = -0.1;
+    result &= result && nav_ptr->setTwistLinear(linear);
+    robot::log_info("setTwistLinear Backward %f", linear.x);
+    return result;
   }
   catch (...)
   {

src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp

@@ -206,7 +206,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     // Process index_s with multiple elements
     if (index_s.size() > 1)
     {
-      for (size_t i = 0; i < index_s.size(); ++i)
+      for (size_t i = 1; i < index_s.size(); ++i)
       {
         if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
         {
@@ -219,11 +219,12 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
         double x = cos(global_plan.poses[index_s[i - 1]].pose.theta) * dx + sin(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
         double y = -sin(global_plan.poses[index_s[i - 1]].pose.theta) * dx + cos(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
 
-        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2)
         {
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
           {
+
             if (index_s[i] > sub_goal_index_saved_)
             {
               sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1;
@@ -257,7 +258,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
         double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[sub_goal_index].pose.theta);
         double x = cos(global_plan.poses[sub_goal_index].pose.theta) * dx + sin(global_plan.poses[sub_goal_index].pose.theta) * dy;
         double y = -sin(global_plan.poses[sub_goal_index].pose.theta) * dx + cos(global_plan.poses[sub_goal_index].pose.theta) * dy;
-        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2)
         {
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
@@ -415,7 +416,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   robot_nav_2d_msgs::Pose2DStamped sub_pose;
   sub_pose = global_plan.poses[closet_index];
 
-#ifdef SCORE_ALGORITHM_WITH_ROS
+#ifdef BUILD_WITH_ROS
   {
     robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose);
     geometry_msgs::PoseStamped sub_pose_stamped_ros;
@@ -423,7 +424,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     sub_pose_stamped_ros.header.frame_id = sub_pose_stamped.header.frame_id;
     sub_pose_stamped_ros.pose.position.x = sub_pose_stamped.pose.position.x;
     sub_pose_stamped_ros.pose.position.y = sub_pose_stamped.pose.position.y;
-    sub_pose_stamped_ros.pose.position.z = sub_pose_stamped.pose.position.z;
+    sub_pose_stamped_ros.pose.position.z = 0.9;
     sub_pose_stamped_ros.pose.orientation.x = sub_pose_stamped.pose.orientation.x;
     sub_pose_stamped_ros.pose.orientation.y = sub_pose_stamped.pose.orientation.y;
     sub_pose_stamped_ros.pose.orientation.z = sub_pose_stamped.pose.orientation.z;
@@ -434,7 +435,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
 
   robot_nav_2d_msgs::Pose2DStamped sub_goal;
   sub_goal = global_plan.poses[goal_index];
-#ifdef SCORE_ALGORITHM_WITH_ROS
+#ifdef BUILD_WITH_ROS
   {
     robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal);
     geometry_msgs::PoseStamped sub_goal_stamped_ros;
@@ -442,7 +443,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     sub_goal_stamped_ros.header.frame_id = sub_goal_stamped.header.frame_id;
     sub_goal_stamped_ros.pose.position.x = sub_goal_stamped.pose.position.x;
     sub_goal_stamped_ros.pose.position.y = sub_goal_stamped.pose.position.y;
-    sub_goal_stamped_ros.pose.position.z = sub_goal_stamped.pose.position.z;
+    sub_goal_stamped_ros.pose.position.z = 0.9;
     sub_goal_stamped_ros.pose.orientation.x = sub_goal_stamped.pose.orientation.x;
     sub_goal_stamped_ros.pose.orientation.y = sub_goal_stamped.pose.orientation.y;
     sub_goal_stamped_ros.pose.orientation.z = sub_goal_stamped.pose.orientation.z;

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -181,7 +181,7 @@ namespace mkt_algorithm
        */
       robot_nav_2d_msgs::Path2D generateTrajectory(  
         const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
-        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd);
+        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd, const double &dt);
 
       /**
        * @brief Generate trajectory

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp

@@ -131,19 +131,19 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
   robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
   
-  // === Final Heading Alignment Check ===
+//   // === Final Heading Alignment Check ===
-  double xy_error = 0.0, heading_error = 0.0;
+//   double xy_error = 0.0, heading_error = 0.0;
-  if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
+//   if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
-  {
+//   {
-    // Use Arc Motion controller for final heading alignment
+//     // Use Arc Motion controller for final heading alignment
-    alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
+//     alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
-#ifdef BUILD_WITH_ROS
+// #ifdef BUILD_WITH_ROS
-    ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+//     ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
-                      xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+//                       xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
-#endif
+// #endif
-  }
+//   }
-  else
+//   else
-  {
+//   {
     // robot::log_info_at(__FILE__, __LINE__, "journey : %f lookahead_dist : %f", 
     //   journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1), lookahead_dist);
     if(fabs(carrot_pose.pose.y) > 0.2)
@@ -151,7 +151,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
       lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
     }
     robot_nav_2d_msgs::Twist2D drive_target;
-    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
     carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
 
     // Normal Pure Pursuit
@@ -164,7 +164,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     sign_x, 
     dt, 
     drive_cmd);
-  }
+  // }
   applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
 
   if (this->nav_stop_)

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -368,6 +368,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
     return false;
   }
+
   const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
   if(fabs(carrot_pose.pose.y) > 0.2)
   {
@@ -440,13 +441,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       double dir_path = cos(fabs(angle_path - goal_pose.theta));
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
-      else
-        x_direction = 0.0;
     }
     catch (std::exception &e)
     {
-      robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
+      robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
-      return false;
+      x_direction = x_direction_; 
     }
   }
 
@@ -483,8 +482,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     twist = traj_->nextTwist();
   }
   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));
+  drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
-  drive_cmd.x = sqrt(twist.x * twist.x);
+  // drive_cmd.x = sqrt(twist.x * twist.x);
   
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
@@ -521,8 +520,12 @@ 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;
+  // robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose;
-  double angle_to_heading;
+  // allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 3.0;
+
+  allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);
+
+  double angle_to_heading; 
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
   {
     if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_))
@@ -551,7 +554,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
 //     else
 //     {
       robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
-      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
       carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
       
       // Normal Pure Pursuit
@@ -578,6 +581,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       result.velocity = drive_cmd;
       return result;
     }
+    
   }
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
@@ -591,7 +595,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
 
   if(fabs(v_max == 0.0))
+  {
     drive_cmd.x = 0.0;
+    robot::log_warning_throttle(0.2, "[%s:%d]\n v_max is 0.0", __FILE__, __LINE__);
+    return result;
+  }
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;
@@ -617,15 +625,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   // 3) Adjust speed using Hermite trajectory curvature + remaining distance
   double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x);
-  const double L_min = 0.1;  // m, chỉnh theo nhu cầu
+  // const double L_min = 0.1;  // m, chỉnh theo nhu cầu
-  double scale_close = std::clamp(L / L_min, 0.0, 1.0);
+  // double scale_close = std::clamp(L / L_min, 0.0, 1.0);
-  v_target *= scale_close;
+  // v_target *= scale_close;
   const double y_abs = std::fabs(carrot_pose.pose.y);
   const double y_soft = 0.1;
   if (y_abs > y_soft) 
   {
     double scale = y_soft / y_abs;  // y càng lớn => scale càng nhỏ
-    scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu
+    scale = std::clamp(scale, 0.6, 1.0); // không giảm quá sâu
     v_target *= scale;
     robot_nav_2d_msgs::Twist2D cmd, result;
     cmd.x = v_target;
@@ -636,9 +644,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   // 4) Maintain minimum approach speed
   if (std::fabs(v_target) < min_approach_linear_velocity)
     v_target = std::copysign(min_approach_linear_velocity, sign_x);
-
-  std::stringstream ss;
-
   // 5) Angular speed from curvature
   double w_target = v_target * kappa;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
@@ -656,7 +661,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
           if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
             continue;
           heading_ref = std::atan2(dy, dx);
-          ss << "error " << heading_ref << " ";
           if(sign_x < 0.0)
           heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
           break;
@@ -664,7 +668,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       }
 
       const double error = heading_ref;
-      ss << error << " ";
       double w_heading = 0.0;
       pid(error, 
         near_goal_heading_integral_, 
@@ -676,8 +679,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
         w_heading);
       // Apply acceleration limits
       double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
-      ss << "dw_heading " << dw_heading << " ";
       w_target = velocity.theta + dw_heading;
+      w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
     }
     else
     {
@@ -689,7 +692,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   {
     near_goal_heading_was_active_ = false;
   }
-  
   w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
 
   // 6) Apply acceleration limits (linear + angular)
@@ -698,7 +700,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
-
+  
   Eigen::VectorXd y(2);
   y << drive_cmd.x, drive_cmd.theta;
 
@@ -717,8 +719,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
   drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
   if (kf_filter_angular_)
-    drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
+    drive_cmd.theta = std::clamp(kf_->state()[3], -fabs(drive_target.theta), fabs(drive_target.theta));
-  // robot::log_info("%s", ss.str().c_str());
+  // robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -742,8 +744,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
     double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
     target_speed = max_speed * cosine_factor;
   }
-
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  double reduce_speed = std::min(max_speed, min_speed_xy_);
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
+  double reduce_speed = std::min(max_speed, v_min);
   if (s < S_final)
   {
     double r = std::clamp(s / S_final, 0.0, 1.0);
@@ -776,10 +779,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   // const double max_kappa = calculateMaxKappa(global_plan);
   // const bool curvature = max_kappa > straight_threshold;
   double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-  if(is_stopped && global_plan.poses.size() >= 2)
+  if(is_stopped && global_plan.poses.size() >= 4 &&
+  journey(global_plan.poses, 0, global_plan.poses.size() - 1) >= 0.7 * min_lookahead_dist_)
   {
-    const auto& p1 = global_plan.poses[1];
+    const auto& p1 = global_plan.poses[2];
-    for(int i = 2; i < global_plan.poses.size(); i++)
+    for(int i = 3; i < global_plan.poses.size(); i++)
     {
       const auto& p = global_plan.poses[i];
       const auto& dx = p.pose.x - p1.pose.x;
@@ -814,12 +818,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
 #ifdef BUILD_WITH_ROS
   if (result)
     ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
-
+#else
-  // else if(fabs(velocity.x) < min_speed_xy_)
+  if (result)
-  // {
+    robot::log_info_throttle(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
-  //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
-  //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
-  // }
 #endif
   return result;
 }
@@ -956,8 +957,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   // --- Linear velocity calculation ---
   // Base velocity proportional to distance, with minimum for smooth motion
   double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
   v_base = std::max(v_base, final_heading_min_velocity_);
-  v_base = std::min(v_base, min_speed_xy_);
+  v_base = std::min(v_base, v_min);
 
   // Scale down when heading error is large (prioritize rotation)
   double heading_scale = 1.0;
@@ -1029,7 +1032,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   cmd_vel.theta = omega_current + domega;
 
   // --- Apply velocity limits ---
-  cmd_vel.x = std::clamp(cmd_vel.x, -min_speed_xy_, min_speed_xy_);
+  cmd_vel.x = std::clamp(cmd_vel.x, -v_min, v_min);
   cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_);
 
   // --- Safety: ensure we can stop ---
@@ -1211,9 +1214,11 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   double v_limit = std::fabs(v_target);
   double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
   
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
   if (journey_distance < min_journey_squared_)
   {
-    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x;
+    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, v_min) * sign_x;
   }
 
   if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
@@ -1223,7 +1228,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   }
 
   if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_)
-    v_limit = min_speed_xy_ * sign_x;
+    v_limit = v_min * sign_x;
 
   if (fabs(decel_lim_x_) > 1e-6)
   {
@@ -1241,7 +1246,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   const robot_nav_2d_msgs::Twist2D &drive_target,
   const robot_nav_2d_msgs::Twist2D &velocity,
   const double &sign_x,
-  robot_nav_2d_msgs::Twist2D &drive_cmd)
+  robot_nav_2d_msgs::Twist2D &drive_cmd, 
+  const double &dt)
 {
   if (path.poses.empty())
   {
@@ -1249,23 +1255,30 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
     drive_cmd.theta = 0.0;
     return robot_nav_2d_msgs::Path2D();
   }
+
+  drive_cmd.x = drive_target.x;
+  drive_cmd.theta = max_vel_theta_;
   double max_kappa = calculateMaxKappa(path);
   const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
-  drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
+  // nếu đường thẳng
-  drive_cmd.theta = max_vel_theta_;
+  if (max_kappa <= straight_threshold) 
-
-  if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
   { 
-    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_ )
+    if(fabs(path.poses.back().pose.x) < min_lookahead_dist_ * 0.8)
     {
+      if(fabs(path.poses.back().pose.x) < min_journey_squared_)
+        drive_cmd.theta = 0.01;
       return generateParallelPath(path, sign_x);
     }
     return generateHermiteTrajectory(path, sign_x);
   }
   else // nếu đường cong
   {
-    if(fabs(drive_cmd.x) < min_speed_xy_)
+    const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-      drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
+    const double v_min = std::min(fabs(v_limited), min_speed_xy_);
+    if(fabs(drive_cmd.x) < v_min) 
+    {
+      drive_cmd.x = std::copysign(v_min, sign_x);
+    }
     return generateHermiteQuadraticTrajectory(path, sign_x);
   }
 }
@@ -1310,8 +1323,6 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
   return parallel_path;
 }
 
-
-
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
   const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {