update 2/2

config/move_base_common_params.yaml

@@ -1,12 +1,12 @@
 
 ### replanning
-controller_frequency: 20.0   # run controller at 15.0 Hz
+controller_frequency: 30.0   # run controller at 15.0 Hz
 controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
 planner_frequency: 0.0       # don't continually replan (only when controller failed)
-planner_patience: 5.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
+planner_patience: 2.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
 max_planning_retries: 5     # ... or after 10 attempts (whichever happens first)
 oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
-oscillation_distance: 0.4
+oscillation_distance: 0.5
 ### recovery behaviors
 recovery_behavior_enabled: true
 recovery_behaviors: [       

config/pnkx_local_planner_params.yaml

@@ -1,6 +1,6 @@
 yaw_goal_tolerance: 0.017  
-xy_goal_tolerance: 0.03 
+xy_goal_tolerance: 0.02 
-min_approach_linear_velocity: 0.06 
+min_approach_linear_velocity: 0.05 
 
 LocalPlannerAdapter:
   library_path: liblocal_planner_adapter
@@ -48,14 +48,14 @@ LimitedAccelGenerator:
   min_vel_y: 0.0  # diff drive robot
 
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
-  min_speed_xy: 0.1    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
-  max_vel_theta: 0.9    # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  max_vel_theta: 0.7   # max_rot_vel: 1.0  # choose slightly less than the base's capability
-  min_vel_theta: 0.04  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
+  min_vel_theta: 0.07  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
   acc_lim_x: 1.0
   acc_lim_y: 0.0      # diff drive robot
-  acc_lim_theta: 0.9
+  acc_lim_theta: 1.5
   decel_lim_x: -1.0
   decel_lim_y: -0.0
   decel_lim_theta: -1.5
@@ -75,8 +75,8 @@ LimitedAccelGenerator:
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
   avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
+  xy_local_goal_tolerance: 0.02
-  angle_threshold: 0.6
+  angle_threshold: 0.47
   index_samples: 60
   follow_step_path: true
 
@@ -85,12 +85,12 @@ MKTAlgorithmDiffPredictiveTrajectory:
   # only when false:
   lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  min_lookahead_dist: 0.5                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
   max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
   lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
   max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
-  max_lateral_accel: 2.0                      # Max lateral accel for speed reduction on curves (m/s^2)
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
   # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
   use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
@@ -100,7 +100,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
 
   # stoped
   rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.06
   
   # Regulated linear velocity scaling
   use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
@@ -114,20 +114,6 @@ MKTAlgorithmDiffPredictiveTrajectory:
   cost_scaling_dist: 0.2                             # (default: 0.6)
   cost_scaling_gain: 2.0                             # (default: 1.0)
 
-
-  use_mpc: true
-  mpc_horizon: 10
-  mpc_dt: 0.1
-  mpc_w_pos: 6.0
-  mpc_w_theta: 2.0
-  mpc_w_v: 0.2
-  mpc_w_w: 0.2
-  mpc_w_dv: 0.4
-  mpc_w_dw: 0.4
-  mpc_iterations: 3
-  mpc_step: 0.15
-  mpc_eps: 0.001
-
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
   avoid_obstacles: false

src/Algorithms/Cores/score_algorithm/README.md

@@ -0,0 +1,44 @@
+# score_algorithm
+
+## Giới thiệu
+`score_algorithm` là lớp nền (base class) cho các thuật toán đánh giá và sinh lệnh điều khiển (local planning) trong `pnkx_nav_core`. Module này không tự sinh quỹ đạo cụ thể, mà cung cấp bộ khung và các hàm dùng chung để các thuật toán con triển khai.
+
+## Vai trò chính
+- Chuẩn hóa giao diện (interface) cho các thuật toán tính toán quỹ đạo/lệnh.
+- Cắt đoạn `global_plan` thành đoạn con phù hợp với `local costmap`.
+- Chọn `sub-goal` và `start` để theo đuổi theo từng bước đường đi.
+- Cung cấp các hàm tiện ích: kiểm tra dừng/đi, tính quãng đường, tìm mục tiêu theo góc và khoảng cách.
+
+## API chính
+Được khai báo trong `include/score_algorithm/score_algorithm.h`:
+- `initialize(...)`: khởi tạo tham số, TF, costmap, bộ sinh quỹ đạo.
+- `prepare(...)`: chuẩn bị dữ liệu dùng chung trước khi đánh giá.
+- `calculator(...)`: tính toán ra `mkt_msgs::Trajectory2D`.
+- `computePlanCommand(...)`: cắt `global_plan` thành đoạn kết quả và chọn `start/goal`.
+- `findSubGoalIndex(...)`, `getGoalIndex(...)`, `journey(...)`, `stopped(...)`: hàm phụ trợ.
+
+## Nguyên lý `computePlanCommand(...)`
+Hàm này nhận `robot_pose`, `velocity`, độ dài xem trước `S` và `global_plan`, sau đó:
+1) Tìm các mốc `sub-goal` (đổi hướng / điểm gãy) bằng `findSubGoalIndex`.
+2) Khôi phục `sub_goal_index` và `sub_start_index` đã lưu (nếu có), cập nhật nếu robot đang dừng và đã vượt mốc.
+3) Tìm `start_index` gần nhất với robot trong đoạn con hợp lệ.
+4) Tìm `goal_index` dựa trên quy tắc "đi đủ xa `S` hoặc đổi hướng vượt ngưỡng".
+5) Xuất `result` là đoạn từ `closet_index` đến `goal_index`.
+
+Kết quả được sử dụng bởi các local planner để sinh lệnh điều khiển tiếp theo.
+
+## Sử dụng trong hệ thống
+`score_algorithm` được nạp qua pluginlib trong các local planner, ví dụ:
+- `pnkx_local_planner`
+- `pnkx_go_straight_local_planner`
+- `pnkx_rotate_local_planner`
+- `pnkx_docking_local_planner`
+
+Mỗi thuật toán cụ thể sẽ kế thừa `ScoreAlgorithm` và triển khai `initialize`, `prepare`, `calculator`.
+
+## Thư mục liên quan
+- `include/score_algorithm/score_algorithm.h`: giao diện chính.
+- `src/score_algorithm.cpp`: các hàm dùng chung (getGoalIndex, computePlanCommand, ...).
+
+## Ghi chú
+Module này phụ thuộc vào `robot_nav_2d_msgs`, `robot_costmap_2d`, `mkt_msgs` và hệ thống TF/ROS.

src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h

@@ -1,6 +1,8 @@
 #ifndef _SCORE_ALGORITHM_H_INCLUDED__
 #define _SCORE_ALGORITHM_H_INCLUDED__
 
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <robot/robot.h>
 #include <iostream>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
@@ -153,11 +155,19 @@ namespace score_algorithm
     double old_xy_goal_tolerance_;
     double angle_threshold_;
 
+    ros::Publisher current_goal_pub_;
+    ros::Publisher closet_robot_goal_pub_;
 
     std::vector<robot_nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
     std::vector<unsigned int> start_index_saved_vt_;
     unsigned int sub_goal_index_saved_, sub_goal_seq_saved_;
     unsigned int sub_start_index_saved_, sub_start_seq_saved_;
+
+    double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
+    double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
+    double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
+    double min_speed_xy_, max_speed_xy_;
+    double rot_stopped_velocity_, trans_stopped_velocity_;
   };
 } // namespace score_algorithm
 

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

@@ -1,3 +1,4 @@
+#include <robot/robot.h>
 #include <score_algorithm/score_algorithm.h>
 #include <robot_nav_2d_utils/path_ops.h>
 #include <robot_nav_2d_utils/conversions.h>
@@ -137,7 +138,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   result.poses.clear();
   if (global_plan.poses.empty())
   {
-     std::cerr << "ScoreAlgorithm: The global plan was empty." << std::endl; 
+    robot::log_error("ScoreAlgorithm: The global plan was empty.");
     return false;
   }
 
@@ -145,7 +146,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   std::vector<robot_nav_2d_msgs::Pose2DStamped> mutes;
   if (!this->findSubGoalIndex(global_plan.poses, seq_s, mutes))
   {
-    std::cerr << "ScoreAlgorithm: Cannot find SubGoal Index" << std::endl;
+    robot::log_error("ScoreAlgorithm: Cannot find SubGoal Index");
     return false;
   }
 
@@ -187,21 +188,19 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     }
   }
 
-  // Handle empty seq_s
   if (index_s.empty())
   {
     sub_goal_index = (unsigned int)global_plan.poses.size();
     sub_goal_seq_saved_ = global_plan.poses.back().header.seq;
-    std::cout << "ScoreAlgorithm: seq_s is empty, setting sub_goal_index to " << sub_goal_index << std::endl;
   }
   else
   {
-    // Update sub_goal_index if index_s.front() is greater and valid
+    bool is_stopped = stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_);
-    if (index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size())
+    if (is_stopped && index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size())
     {
-      sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front();
+      // sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front();
+      sub_goal_index = index_s.front();
       sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
-      // std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl;
     }
 
     // Process index_s with multiple elements
@@ -211,7 +210,6 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
       {
         if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
         {
-          std::cout << "ScoreAlgorithm: Invalid index index_s[" << i - 1 << "]=" << index_s[i - 1] << " or index_s[" << i << "]=" << index_s[i] << ", plan size=" << (unsigned int)global_plan.poses.size() << std::endl;
           continue;
         }
 
@@ -226,12 +224,8 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
           {
-            // ROS_INFO_THROTTLE(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[index_s[i - 1]].pose.theta);
-            // ROS_INFO_THROTTLE(0.1, "tolerance 1 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
             if (index_s[i] > sub_goal_index_saved_)
             {
-              // ROS_INFO("%d %d %d %d %d", index_s[i], sub_goal_index, sub_goal_index_saved_, sub_goal_seq_saved_, (unsigned int)global_plan.poses.size());
-              // ROS_INFO_NAMED("ScoreAlgorithm", "Following from %u to %d", sub_goal_index, i < (unsigned int)(index_s.size() - 1) ? index_s[i] + 1 : (unsigned int)global_plan.poses.size());
               sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1;
               sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
               old_xy_goal_tolerance_ = 0.0;
@@ -255,7 +249,6 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
       {
         sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
         sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
-        // std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl;
       }
       else
       {
@@ -269,12 +262,10 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
           {
-            // ROS_INFO_THROTTLE(0.1, "%f %f %f %f ", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[sub_goal_index].pose.theta);
-            // ROS_INFO_THROTTLE(0.1, "tolerance 2 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
             sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
             sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
             old_xy_goal_tolerance_ = 0.0;
-            std::cout << "ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to " << sub_goal_index << std::endl;
+            robot::log_info("ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to %d", sub_goal_index);
           }
         }
         else
@@ -284,7 +275,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   }
   if (sub_start_index >= sub_goal_index)
   {
-    std::cerr << "ScoreAlgorithm: Sub path is empty" << std::endl;
+    robot::log_error("ScoreAlgorithm: Sub path is empty");
     return false;
   }
 
@@ -310,7 +301,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   // Nếu khồng tìm được điểm gần với robot nhất thì trả về false
   if (!started_path)
   {
-    std::cerr << "ScoreAlgorithm: None of the points of the global plan were in the local costmap." << std::endl;
+    robot::log_error("ScoreAlgorithm: None of the points of the global plan were in the local costmap.");
     return false;
   }
   std::stringstream ss;
@@ -369,17 +360,9 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     // check if goal already reached
     bool goal_already_reached = false;
 
-    // robot_geometry_msgs::PoseArray poseArrayMsg;
-    // poseArrayMsg.header.stamp = robot::Time::now();
-    // poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
-    // int c = 0;
     for (auto &reached_intermediate_goal : reached_intermediate_goals_)
     {
       double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose));
-
-      // robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose);
-      // poseArrayMsg.poses.push_back(pose);
-
       if (distance < xy_local_goal_tolerance_)
       {
         goal_already_reached = true;
@@ -403,7 +386,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
       {
         // the robot has currently reached the goal
         reached_intermediate_goals_.push_back(global_plan.poses[goal_index]);
-        std::cout << "ScoreAlgorithm: Number of reached_intermediate goals: " << reached_intermediate_goals_.size() << std::endl;
+        robot::log_info("ScoreAlgorithm: Number of reached_intermediate goals: %d", reached_intermediate_goals_.size());
       }
       else
       {
@@ -432,8 +415,37 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   robot_nav_2d_msgs::Pose2DStamped sub_pose;
   sub_pose = global_plan.poses[closet_index];
 
+  {
+    robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose);
+    geometry_msgs::PoseStamped sub_pose_stamped_ros;
+    sub_pose_stamped_ros.header.stamp = ros::Time::now();
+    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.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;
+    sub_pose_stamped_ros.pose.orientation.w = sub_pose_stamped.pose.orientation.w;
+    closet_robot_goal_pub_.publish(sub_pose_stamped_ros);
+  }
+
   robot_nav_2d_msgs::Pose2DStamped sub_goal;
   sub_goal = global_plan.poses[goal_index];
+  {
+    robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal);
+    geometry_msgs::PoseStamped sub_goal_stamped_ros;
+    sub_goal_stamped_ros.header.stamp = ros::Time::now();
+    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.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;
+    sub_goal_stamped_ros.pose.orientation.w = sub_goal_stamped.pose.orientation.w;
+    current_goal_pub_.publish(sub_goal_stamped_ros);
+  }
 
   result.header = global_plan.header;
   for (int i = closet_index; i <= goal_index; i++)

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

@@ -144,16 +144,65 @@ namespace mkt_algorithm
           const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
           robot_nav_2d_msgs::Path2D &transformed_plan);
 
-      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose);
+      /**
+       * @brief Calculate max kappa
+       * @param path
+       * @param preview_distance
+       * @return max kappa
+       */
+      double calculateMaxKappa(const robot_nav_2d_msgs::Path2D &path, const double preview_distance = std::numeric_limits<double>::max());
+    
+      /**
+       * @brief Adjust speed with Hermite trajectory
+       * @param velocity
+       * @param trajectory
+       * @param v_target
+       * @param sign_x
+       * @return speed
+       */
+      double adjustSpeedWithHermiteTrajectory(
+        const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Path2D &trajectory, double v_target, const double &sign_x
+      );
+
+      /**
+       * @brief Generate Hermite trajectory
+       * @param path
+       * @param drive_target
+       * @param velocity
+       * @param sign_x
+       * @param drive_cmd
+      * @return trajectory
+       */
+      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);
+
+      /**
+       * @brief Generate Hermite trajectory
+       * @param pose
+       * @param sign_x
+       * @return trajectory
+       */
+      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
+
+      /**
+       * @brief Generate Hermite quadratic trajectory
+       * @param pose
+       * @param sign_x
+       * @return trajectory
+       */
+      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
 
       /**
        * @brief Should rotate to path
        * @param carrot_pose
+       * @param velocity
+       * @param sign_x
        * @param angle_to_path
        * @return true if should rotate, false otherwise
        */
       bool shouldRotateToPath(
-          const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x);
+          const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, robot_nav_2d_msgs::Twist2D velocity, double &angle_to_path, const double &sign_x);
 
       /**
        * @brief Rotate to heading
@@ -163,6 +212,40 @@ namespace mkt_algorithm
        */
       void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
 
+      /**
+       * @brief Check if robot should align to final heading (near goal position)
+       * @param trajectory The transformed trajectory in robot frame
+       * @param goal The goal pose with desired final heading
+       * @param velocity Current velocity
+       * @param xy_error Output: distance to goal
+       * @param heading_error Output: angle difference to goal heading
+       * @return true if should enter final heading alignment phase
+       */
+      bool shouldAlignToFinalHeading(
+          const robot_nav_2d_msgs::Path2D &trajectory,
+          const robot_nav_2d_msgs::Pose2DStamped &goal,
+          const robot_nav_2d_msgs::Twist2D &velocity,
+          double &xy_error,
+          double &heading_error);
+
+      /**
+       * @brief Arc Motion controller for final heading alignment
+       * Smoothly reduces linear velocity while adjusting angular velocity to reach final heading
+       * @param xy_error Distance to goal position
+       * @param heading_error Angle difference to goal heading
+       * @param velocity Current velocity
+       * @param sign_x Direction of motion (forward/backward)
+       * @param dt Time step
+       * @param cmd_vel Output velocity command
+       */
+      void alignToFinalHeading(
+          const double &xy_error,
+          const double &heading_error,
+          const robot_nav_2d_msgs::Twist2D &velocity,
+          const double &sign_x,
+          const double &dt,
+          robot_nav_2d_msgs::Twist2D &cmd_vel);
+
       /**
        * @brief  the robot is moving acceleration limits
        * @param  velocity The velocity of the robot
@@ -182,20 +265,16 @@ namespace mkt_algorithm
       bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
 
       /**
-       * @brief Apply constraints
+       * @brief Compute pure pursuit
-       * @param dist_error
+       * @param carrot_pose
-       * @param lookahead_dist
+       * @param drive_target
-       * @param curvature
+       * @param velocity
-       * @param curr_speed
+       * @param trajectory
-       * @param pose_cost
+       * @param min_approach_linear_velocity
-       * @param linear_vel
+       * @param sign_x
-       * @param sign
+       * @param dt
+       * @param drive_cmd
        */
-      void applyConstraints(
-          const double &dist_error, const double &lookahead_dist,
-          const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
-          const double &pose_cost, double &linear_vel, const double &sign_x);
-
       void computePurePursuit(
         const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
         const robot_nav_2d_msgs::Twist2D &drive_target,
@@ -206,14 +285,15 @@ namespace mkt_algorithm
         const double &dt,
         robot_nav_2d_msgs::Twist2D &drive_cmd);
 
-      double adjustSpeedWithHermiteTrajectory(
+      /**
-        const robot_nav_2d_msgs::Twist2D &velocity,
+       * @brief Interpolate footprint
-        const robot_nav_2d_msgs::Path2D &trajectory,
+       * @param pose
-        double v_target,
+       * @param footprint
-        const double &sign_x
+       * @param resolution
-      );
+       * @return interpolated footprint
-
+       */
-      std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
+      std::vector<robot_geometry_msgs::Point> interpolateFootprint(
+        const robot_geometry_msgs::Pose2D &pose, const std::vector<robot_geometry_msgs::Point> &footprint, const double &resolution);
 
       /**
        * @brief Cost at pose
@@ -223,11 +303,6 @@ namespace mkt_algorithm
        */
       double costAtPose(const double &x, const double &y);
 
-      double computeDecelerationFactor(double remaining_distance, double decel_distance);
-
-      double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);
-
-      double estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan);
 
       bool initialized_;
       bool nav_stop_;
@@ -258,17 +333,17 @@ namespace mkt_algorithm
       // Rotate to heading
       bool use_rotate_to_heading_;
       double rotate_to_heading_min_angle_;
-
-      double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
       double min_path_distance_, max_path_distance_;
 
+      // Final heading alignment (Arc Motion)
+      bool use_final_heading_alignment_;
+      double final_heading_xy_tolerance_;      // Distance threshold to trigger alignment
+      double final_heading_angle_tolerance_;   // Angle threshold to consider heading reached
+      double final_heading_min_velocity_;      // Minimum linear velocity during alignment
+      double final_heading_kp_angular_;        // Proportional gain for angular control
+
       // Regulated linear velocity scaling
       bool use_regulated_linear_velocity_scaling_;
-      double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
-      double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
-      double min_speed_xy_, max_speed_xy_;
-
-      double rot_stopped_velocity_, trans_stopped_velocity_;
 
       double min_approach_linear_velocity_;
       double regulated_linear_scaling_min_radius_;
@@ -291,6 +366,8 @@ namespace mkt_algorithm
       Eigen::MatrixXd R;
       Eigen::MatrixXd P;
 
+      ros::Publisher lookahead_point_pub_;
+
     }; // class PredictiveTrajectory
 
   } // namespace diff

src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md

@@ -30,16 +30,21 @@ h11 =  t^3 - t^2
 ```
 
 ### Đạo hàm đầu/cuối
-Chọn độ dài đặc trưng `L` (thường L = sqrt(x^2 + y^2)):
+Chọn độ dài đặc trưng `L` (thường L = sqrt(x^2 + y^2)) và hướng `dir`:
 ```
-p'(0) = (L, 0)
+dir = +1  (đi xuôi)
-p'(1) = (L*cos(theta), L*sin(theta))
+dir = -1  (đi ngược 180° ở cả đầu và cuối)
+```
+Khi đó:
+```
+p'(0) = (dir * L, 0)
+p'(1) = (dir * L*cos(theta), dir * L*sin(theta))
 ```
 
 ### Công thức quỹ đạo
 ```
-x(t) = h10*L + h01*x + h11*L*cos(theta)
+x(t) = h10*(dir*L) + h01*x + h11*(dir*L*cos(theta))
-y(t) = h01*y + h11*L*sin(theta)
+y(t) = h01*y + h11*(dir*L*sin(theta))
 ```
 
 ### Hướng (heading) trên đường cong
@@ -50,8 +55,8 @@ h10' = 3t^2 - 4t + 1
 h01' = -6t^2 + 6t
 h11' = 3t^2 - 2t
 
-dx = h10'*L + h01'*x + h11'*L*cos(theta)
+dx = h10'*(dir*L) + h01'*x + h11'*(dir*L*cos(theta))
-dy = h01'*y + h11'*L*sin(theta)
+dy = h01'*y + h11'*(dir*L*sin(theta))
 ```
 Hướng:
 ```

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

@@ -104,12 +104,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
 
   robot_nav_2d_msgs::Twist2D twist;
   traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
-  robot::Rate r(50);
   while (robot::ok() && traj_->hasMoreTwists())
   {
     twist = traj_->nextTwist();
-    // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
-    r.sleep();
   }
   
   drive_cmd.x = sqrt(twist.x * twist.x);
@@ -120,105 +117,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     return result;
   }
 
-  // Dynamically adjust look ahead distance based on the speed
-  double lookahead_dist = this->getLookAheadDistance(twist);
-  // Return false if the transformed global plan is empty
-  robot_geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
-  lookahead_dist += fabs(front.y);
 
-  // Get lookahead point and publish for visualization
-  auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
-
-  // Carrot distance squared
-  const double carrot_dist2 =
-      (carrot_pose.pose.x * carrot_pose.pose.x) +
-      (carrot_pose.pose.y * carrot_pose.pose.y);
-
-  // Find curvature of circle (k = 1 / R)
-  double curvature = 0.0;
-  if (carrot_dist2 > 2e-2)
-  {
-    curvature = 2.0 * carrot_pose.pose.y / carrot_dist2;
-  }
-
-  // Constrain linear velocity
-  this->applyConstraints(
-      0.0, lookahead_dist, curvature, twist,
-      this->costAtPose(pose.pose.x, pose.pose.y),
-      drive_cmd.x, sign_x);
-
-
-  if (std::hypot(carrot_pose.pose.x, carrot_pose.pose.y) > min_journey_squared_)
-  {
-    robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
-    robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
-    double dx = end.pose.x - start.pose.x;
-    double dy = end.pose.y - start.pose.y;
-    drive_cmd.theta = atan2(dy, dx);
-
-    //[-π/2, π/2]
-    if (drive_cmd.theta > M_PI / 2)
-      drive_cmd.theta -= M_PI;
-    else if (drive_cmd.theta < -M_PI / 2)
-      drive_cmd.theta += M_PI;
-
-    drive_cmd.theta = std::clamp(drive_cmd.theta, -min_vel_theta_, min_vel_theta_);
-  }
-  else
-    drive_cmd.theta = 0.0;
-
-  // populate and return message
-  if (fabs(carrot_pose.pose.x) > 1e-9 || carrot_pose.pose.y > 1e-9)
-    drive_cmd.x *= fabs(cos(std::atan2(carrot_pose.pose.y, carrot_pose.pose.x)));
-    
-  double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  const double vel_x_reduce = std::min(fabs(v_max), 0.3);
-  double journey_plan = compute_plan_.poses.empty() ? 0 : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
-  const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
-  const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
-  double d_reduce = std::clamp(journey_plan, min_S, max_S);
-  double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
-  double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
-  double v_min = 
-    journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
-  v_min *= sign_x;
-  
-  double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
-  double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
-  journey_plan += max_path_distance_;
-  drive_cmd.x = journey_plan >= d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * decel_factor + v_min;
-
-  // drive_cmd.x = journey_plan > d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * sin((journey_plan / d_reduce) * (M_PI / 2)) + v_min;
-
-  Eigen::VectorXd y(2);
-  y << drive_cmd.x, drive_cmd.theta;
-
-  // Cập nhật lại A nếu dt thay đổi
-  for (int i = 0; i < n_; ++i)
-    for (int j = 0; j < n_; ++j)
-      A(i, j) = (i == j ? 1.0 : 0.0);
-  for (int i = 0; i < n_; i += 3)
-  {
-    A(i, i + 1) = dt;
-    A(i, i + 2) = 0.5 * dt * dt;
-    A(i + 1, i + 2) = dt;
-  }
-  kf_->update(y, dt, A);
-
-  if (drive_cmd.x != v_max)
-  {
-    double min = drive_cmd.x < v_max ? drive_cmd.x : v_max;
-    double max = drive_cmd.x > v_max ? drive_cmd.x : v_max;
-    drive_cmd.x = std::clamp(kf_->state()[0], min, max);
-  }
-  else
-  {
-    drive_cmd.x = std::clamp(kf_->state()[0], v_max, v_max);
-  }
-
-  drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
-  drive_cmd.theta = kf_->state()[3];
-  result.velocity = drive_cmd;
   return result;
 }
 

src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp

@@ -194,7 +194,6 @@ namespace mkt_plugins
   void KinematicParameters::setMaxTheta(double value)
   {
     max_vel_theta_  = fabs(value) <= fabs(original_max_vel_theta_) + EPSILON? value : original_max_vel_theta_;
-    // ROS_WARN_THROTTLE(10, "vtheta %f %f %f", value, original_max_vel_theta_, max_vel_theta_);
   }
 
   void KinematicParameters::setAccTheta(double value)

src/Algorithms/Packages/global_planners/custom_planner

@@ -1 +1 @@
-Subproject commit 49afcce5b2247793282fe5e94d0e27f062562325
+Subproject commit e0f6738c3161b7a4a9418d960d7e419a43f7d521

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -221,7 +221,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     try
     {
       robot::PluginLoaderHelper loader;
-      std::string path_file_so = loader.findLibraryPath("CustomPlanner");
+      std::string path_file_so = loader.findLibraryPath(global_planner);
       planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, global_planner, boost::dll::load_mode::append_decorations);
 
@@ -257,7 +257,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     try
     {
       robot::PluginLoaderHelper loader;
-      std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter");
+      std::string path_file_so = loader.findLibraryPath(local_planner);
       controller_loader_ =
           boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
               path_file_so, local_planner, boost::dll::load_mode::append_decorations);