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,9 +1,9 @@
+yaw_goal_tolerance: 0.017  
+xy_goal_tolerance: 0.02 
+min_approach_linear_velocity: 0.05 
+
 LocalPlannerAdapter:
   library_path: liblocal_planner_adapter
-  yaw_goal_tolerance: 0.017  
-  xy_goal_tolerance: 0.03 
-  min_approach_linear_velocity: 0.06   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
-
 PNKXLocalPlanner:
   # Algorithm
   library_path: libpnkx_local_planner
@@ -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
-  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
+  max_vel_theta: 0.7   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  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
-  angle_threshold: 0.4
+  xy_local_goal_tolerance: 0.02
+  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.4                     # 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: 2.0                         # 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) 
+  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  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)
@@ -99,8 +99,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.1
-  trans_stopped_velocity: 0.1
+  rot_stopped_velocity: 0.05
+  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
-    if (index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size())
+    bool is_stopped = stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_);
+    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/CMakeLists.txt

@@ -81,10 +81,9 @@ endif()
 # Libraries
 # ========================================================
 add_library(${PROJECT_NAME}_diff SHARED
-  src/diff/diff_predictive_trajectory_.cpp
+  src/diff/diff_predictive_trajectory.cpp
   src/diff/diff_rotate_to_goal.cpp
   src/diff/diff_go_straight.cpp
-  # src/diff/pure_pursuit.cpp
 )
 
 if(BUILDING_WITH_CATKIN)

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

@@ -113,13 +113,6 @@ namespace mkt_algorithm
       std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
       getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);
 
-      /**
-       * @brief Create carrot message
-       * @param carrot_pose
-       * @return carrot message
-       */
-      robot_geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose);
-
       /**
        * @brief Prune global plan
        * @param tf
@@ -151,14 +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);
 
+      /**
+       * @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
@@ -168,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
@@ -187,21 +265,35 @@ 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
-       * @param dist_error
-       * @param lookahead_dist
-       * @param curvature
-       * @param curr_speed
-       * @param pose_cost
-       * @param linear_vel
-       * @param sign
+       * @brief Compute pure pursuit
+       * @param carrot_pose
+       * @param drive_target
+       * @param velocity
+       * @param trajectory
+       * @param min_approach_linear_velocity
+       * @param sign_x
+       * @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, double &sign_x);
+      void computePurePursuit(
+        const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
+        const robot_nav_2d_msgs::Twist2D &drive_target,
+        const robot_nav_2d_msgs::Twist2D &velocity,
+        const robot_nav_2d_msgs::Path2D &trajectory,
+        const double &min_approach_linear_velocity,
+        const double &sign_x,
+        const double &dt,
+        robot_nav_2d_msgs::Twist2D &drive_cmd);
 
-      std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
+      /**
+       * @brief Interpolate footprint
+       * @param pose
+       * @param footprint
+       * @param resolution
+       * @return interpolated footprint
+       */
+      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
@@ -211,21 +303,6 @@ namespace mkt_algorithm
        */
       double costAtPose(const double &x, const double &y);
 
-      void updateCostAtOffset(
-          TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose,
-          double x_offset, double y_offset, double &cost_left, double &cost_right);
-
-      double computeDecelerationFactor(double remaining_distance, double decel_distance);
-
-      double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);
-
-      bool detectWobbleByCarrotAngle(std::vector<double>& angle_history, double current_angle, 
-                                    double amplitude_threshold = 0.3, size_t window_size = 20);
-
-      void publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose);
-
-      std::vector<double> angle_history_;
-      size_t window_size_;
 
       bool initialized_;
       bool nav_stop_;
@@ -240,8 +317,6 @@ namespace mkt_algorithm
       robot_nav_2d_msgs::Twist2D prevous_drive_cmd_;
 
       double x_direction_, y_direction_, theta_direction_;
-      double max_robot_pose_search_dist_;
-      double global_plan_prune_distance_{1.0};
 
       // Lookahead
       bool use_velocity_scaled_lookahead_dist_;
@@ -249,6 +324,7 @@ namespace mkt_algorithm
       double lookahead_dist_;
       double min_lookahead_dist_;
       double max_lookahead_dist_;
+      double max_lateral_accel_;
 
       // journey
       double min_journey_squared_{1e9};
@@ -257,16 +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 rot_stopped_velocity_, trans_stopped_velocity_;
 
       double min_approach_linear_velocity_;
       double regulated_linear_scaling_min_radius_;
@@ -276,10 +353,6 @@ namespace mkt_algorithm
       double inflation_cost_scaling_factor_;
       double cost_scaling_dist_, cost_scaling_gain_;
 
-      double cost_left_goal_, cost_right_goal_;
-      double cost_left_side_ , cost_right_side_;
-      double center_cost_;
-
       // Control frequency
       double control_duration_;
       std::vector<robot_geometry_msgs::Point> footprint_spec_;
@@ -293,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/include/mkt_algorithm/diff/diff_predictive_trajectory_.h

@@ -1,314 +0,0 @@
-#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
-#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
-
-#include <robot/robot.h>
-#include <score_algorithm/score_algorithm.h>
-#include <robot_geometry_msgs/PoseStamped.h>
-#include <robot_geometry_msgs/PointStamped.h>
-#include <robot_nav_2d_msgs/Pose2DStamped.h>
-#include <robot_nav_core2/exceptions.h>
-#include <robot_nav_core2/costmap.h>
-#include <nav_grid/coordinate_conversion.h>
-#include <angles/angles.h>
-#include <robot_nav_msgs/Path.h>
-#include <kalman/kalman.h>
-#include <vector>
-
-#include <robot_nav_2d_utils/parameters.h>
-#include <robot_nav_2d_utils/tf_help.h>
-#include <robot_nav_2d_utils/path_ops.h>
-#include <robot_nav_2d_utils/conversions.h>
-
-namespace mkt_algorithm
-{
-  namespace diff
-  {
-    /**
-     * @class PredictiveTrajectory
-     * @brief Standard PredictiveTrajectory-like ScoreAlgorithm
-     */
-    class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
-    {
-    public:
-      PredictiveTrajectory() : initialized_(false), nav_stop_(false) {};
-
-      virtual ~PredictiveTrajectory();
-
-      // Standard ScoreAlgorithm Interface
-      /**
-       * @brief Initialize parameters as needed
-       * @param nh NodeHandle to read parameters from
-       */
-      virtual void initialize(
-        robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
-
-      /**
-       * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
-       *
-       * Subclasses may overwrite. Return false in case there is any error.
-       *
-       * @param pose Current pose (costmap frame)
-       * @param velocity Current velocity
-       * @param goal The final goal (costmap frame)
-       * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
-       */
-      virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
-                           const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
-                           double &x_direction, double &y_direction, double &theta_direction) override;
-
-      /**
-       * @brief Calculating algorithm
-       * @param pose
-       * @param velocity
-       * @param traj
-       */
-      virtual mkt_msgs::Trajectory2D calculator(
-          const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;
-
-      /**
-       * @brief Reset all data
-       */
-      virtual void reset() override;
-
-      /**
-       * @brief Stoping move navigation
-       */
-      virtual void stop() override;
-
-      /**
-       * @brief resume move navigation after stopped
-       */
-      virtual void resume() override;
-
-      /**
-       * @brief Create a new PredictiveTrajectory instance
-       * @return A pointer to the new PredictiveTrajectory instance
-       */
-      static score_algorithm::ScoreAlgorithm::Ptr create();
-
-    protected:
-      inline double sign(double x)
-      {
-        return x < 0.0 ? -1.0 : 1.0;
-      }
-
-      /**
-       * @brief Initialize parameters
-       */
-      virtual void getParams();
-
-      /**
-       * @brief Dynamically adjust look ahead distance based on the speed
-       * @param velocity
-       * @return look ahead distance
-       */
-      double getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity);
-
-      /**
-       * @brief Get lookahead point on the global plan
-       * @param lookahead_dist
-       * @param global_plan
-       * @return lookahead point
-       */
-      std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
-      getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);
-
-      /**
-       * @brief Prune global plan
-       * @param tf
-       * @param pose
-       * @param global_plan
-       * @param dist_behind_robot
-       * @return true if plan is pruned, false otherwise
-       */
-      bool pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose,
-                           robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot);
-
-      /**
-       * @brief  Transforms the global plan of the robot from the planner frame to the local frame (modified).
-       *
-       * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
-       * such that the index of the current goal pose is returned as well as
-       * the transformation between the global plan and the planning frame.
-       * @param tf A reference to a tf buffer
-       * @param global_plan The plan to be transformed
-       * @param pose The pose of the robot
-       * @param costmap A reference to the costmap being used so the window size for transforming can be computed
-       * @param global_frame The frame to transform the plan to
-       * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
-       * @param[out] transformed_plan Populated with the transformed plan
-       * @return \c true if the global plan is transformed, \c false otherwise
-       */
-      bool transformGlobalPlan(
-          TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
-          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 Should rotate to path
-       * @param carrot_pose
-       * @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);
-
-      /**
-       * @brief Rotate to heading
-       * @param  angle_to_path
-       * @param  velocity The velocity of the robot
-       * @param  cmd_vel The velocity commands to be filled
-       */
-      void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
-
-      /**
-       * @brief  the robot is moving acceleration limits
-       * @param  velocity The velocity of the robot
-       * @param  cmd_vel The velocity commands
-       * @param  cmd_vel_result The velocity commands result
-       */
-      void moveWithAccLimits(
-          const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result);
-
-      /**
-       * @brief Stop the robot taking into account acceleration limits
-       * @param  pose The pose of the robot in the global frame
-       * @param  velocity The velocity of the robot
-       * @param  cmd_vel The velocity commands to be filled
-       * @return  True if a valid trajectory was found, false otherwise
-       */
-      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
-       * @param dist_error
-       * @param lookahead_dist
-       * @param curvature
-       * @param curr_speed
-       * @param pose_cost
-       * @param linear_vel
-       * @param sign
-       */
-      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 score_algorithm::TrajectoryGenerator::Ptr &traj,
-        const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
-        const robot_nav_2d_msgs::Twist2D &velocity,
-        const double &min_approach_linear_velocity,
-        const double &journey_plan,
-        const double &sign_x,
-        const double &lookahead_dist_min,
-        const double &lookahead_dist_max,
-        const double &lookahead_dist,
-        const double &lookahead_time,
-        const double &dt,
-        robot_nav_2d_msgs::Twist2D &drive_cmd
-      );
-
-      double adjustSpeedWithHermiteTrajectory(
-        const robot_nav_2d_msgs::Twist2D &velocity,
-        const robot_nav_2d_msgs::Path2D &trajectory,
-        double v_target,
-        const double &sign_x
-      );
-
-      std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
-
-      /**
-       * @brief Cost at pose
-       * @param x
-       * @param y
-       * @return cost
-       */
-      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);
-
-
-      std::vector<double> angle_history_;
-      size_t window_size_;
-
-      bool initialized_;
-      bool nav_stop_;
-      robot::NodeHandle nh_, nh_priv_;
-      std::string frame_id_path_;
-      std::string robot_base_frame_;
-
-      robot_nav_2d_msgs::Pose2DStamped goal_;
-      robot_nav_2d_msgs::Path2D global_plan_;
-      robot_nav_2d_msgs::Path2D compute_plan_;
-      robot_nav_2d_msgs::Path2D transform_plan_;
-      robot_nav_2d_msgs::Twist2D prevous_drive_cmd_;
-
-      double x_direction_, y_direction_, theta_direction_;
-      double max_robot_pose_search_dist_;
-      double global_plan_prune_distance_{1.0};
-
-      // Lookahead
-      bool use_velocity_scaled_lookahead_dist_;
-      double lookahead_time_;
-      double lookahead_dist_;
-      double min_lookahead_dist_;
-      double max_lookahead_dist_;
-      double max_lateral_accel_;
-
-      // journey
-      double min_journey_squared_{1e9};
-      double max_journey_squared_{1e9};
-
-      // 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_;
-
-      // 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 rot_stopped_velocity_, trans_stopped_velocity_;
-
-      double min_approach_linear_velocity_;
-      double regulated_linear_scaling_min_radius_;
-      double regulated_linear_scaling_min_speed_;
-
-      bool use_cost_regulated_linear_velocity_scaling_;
-      double inflation_cost_scaling_factor_;
-      double cost_scaling_dist_, cost_scaling_gain_;
-
-      double cost_left_goal_, cost_right_goal_;
-      double cost_left_side_ , cost_right_side_;
-      double center_cost_;
-
-      // Control frequency
-      double control_duration_;
-      std::vector<robot_geometry_msgs::Point> footprint_spec_;
-
-      robot::Time last_actuator_update_;
-      boost::shared_ptr<KalmanFilter> kf_;
-      int m_, n_;
-      Eigen::MatrixXd A;
-      Eigen::MatrixXd C;
-      Eigen::MatrixXd Q;
-      Eigen::MatrixXd R;
-      Eigen::MatrixXd P;
-
-    }; // class PredictiveTrajectory
-
-  } // namespace diff
-
-} // namespace mkt_algorithm
-
-#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__

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

@@ -59,12 +59,6 @@ namespace mkt_algorithm
        */
       static score_algorithm::ScoreAlgorithm::Ptr create();
 
-    protected:
-      /**
-       * @brief Initialize parameters
-       */
-      virtual void getParams() override;
-
     }; // class RotateToGoalDiff
 
   } // namespace diff

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

@@ -1,52 +0,0 @@
-#ifndef _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
-#define _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
-
-#include <robot/robot.h>
-#include <score_algorithm/score_algorithm.h>
-#include <robot_geometry_msgs/PoseStamped.h>
-#include <robot_geometry_msgs/PointStamped.h>
-#include <robot_nav_2d_msgs/Pose2DStamped.h>
-#include <robot_nav_core2/exceptions.h>
-#include <robot_nav_core2/costmap.h>
-#include <nav_grid/coordinate_conversion.h>
-#include <angles/angles.h>
-
-namespace mkt_algorithm
-{
-  namespace diff
-  {
-    class PurePursuit
-    {
-    public:
-      void computePurePursuit(
-        const score_algorithm::TrajectoryGenerator::Ptr &traj,
-        const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
-        const robot_nav_2d_msgs::Twist2D &velocity,
-        const double &min_approach_linear_velocity,
-        const double &journey_plan,
-        const double &sign_x,
-        const double &lookahead_dist_min,
-        const double &lookahead_dist_max,
-        const double &lookahead_dist,
-        const double &lookahead_time,
-        const double &dt,
-        robot_nav_2d_msgs::Twist2D &drive_cmd
-      );
-
-
-    private:
-      void applyConstraints(const double &dist_error, const double &lookahead_dist,
-                            const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
-                            const double &pose_cost, double &linear_vel, const double &sign_x);
-
-      double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
-                                                      const double &journey_plan);
-
-      double computeDecelerationFactor(const double &effective_journey, const double &d_reduce);
-
-      // properties
-      double max_lateral_accel_;
-    };
-  }
-}
-#endif  

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)
-p'(1) = (L*cos(theta), L*sin(theta))
+dir = +1  (đi xuôi)
+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)
-y(t) = h01*y + h11*L*sin(theta)
+x(t) = h10*(dir*L) + h01*x + h11*(dir*L*cos(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)
-dy = h01'*y + h11'*L*sin(theta)
+dx = h10'*(dir*L) + h01'*x + h11'*(dir*L*cos(theta))
+dy = h01'*y + h11'*(dir*L*sin(theta))
 ```
 Hướng:
 ```

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

@@ -7,15 +7,14 @@ void mkt_algorithm::diff::GoStraight::initialize(
 {
   if (!initialized_)
   {
-    nh_ = robot::NodeHandle("~");
+    nh_ = nh;
     nh_priv_ = robot::NodeHandle(nh, name);
     name_ = name;
     tf_ = tf;
     traj_ = traj;
     costmap_robot_ = costmap_robot;
-
+    nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
     this->getParams();
-    nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
     std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
     if (footprint.size() > 1)
@@ -105,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);
@@ -121,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_algorithm/src/diff/diff_rotate_to_goal.cpp

@@ -14,6 +14,7 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
     tf_ = tf;
     traj_ = traj;
     costmap_robot_ = costmap_robot;
+    nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
     this->getParams();
 
     x_direction_ = y_direction_ = theta_direction_ = 0;
@@ -22,68 +23,13 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
   }
 }
 
-void mkt_algorithm::diff::RotateToGoal::getParams()
-{
-  robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
-  nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
-  nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
-  nh_priv_.param<int>("index_samples", index_samples_, 0);
-  nh_priv_.param<bool>("follow_step_path", follow_step_path_, false);
-
-  nh_priv_.param<bool>("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false);
-  nh_priv_.param<double>("lookahead_dist", lookahead_dist_, 0.25);
-  nh_priv_.param<double>("min_lookahead_dist", min_lookahead_dist_, 0.3);
-  nh_priv_.param<double>("max_lookahead_dist", max_lookahead_dist_, 0.9);
-  nh_priv_.param<double>("lookahead_time", lookahead_time_, 1.5);
-  nh_priv_.param<double>("min_journey_squared", min_journey_squared_, std::numeric_limits<double>::infinity());
-  nh_priv_.param<double>("max_journey_squared", max_journey_squared_, std::numeric_limits<double>::infinity());
-
-  nh_priv_.param<bool>("use_rotate_to_heading", use_rotate_to_heading_, false);
-  nh_priv_.param<double>("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785);
-
-  nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.0);
-  nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.0);
-  nh_priv_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
-
-  // Regulated linear velocity scaling
-  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
-  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
-  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
-
-  // Inflation cost scaling (Limit velocity by proximity to obstacles)
-  nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
-  nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
-  nh_priv_.param<double>("cost_scaling_dist", cost_scaling_dist_, 0.6);
-  nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
-  if (inflation_cost_scaling_factor_ <= 0.0)
-  {
-    robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
-    use_cost_regulated_linear_velocity_scaling_ = false;
-  }
-  double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
-  control_duration_ = 1.0 / control_frequency;
-  if (traj_)
-  {
-    traj_.get()->getNodeHandle().param<double>("max_vel_x", max_vel_x_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("min_vel_x", min_vel_x_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("acc_lim_x", acc_lim_x_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("decel_lim_x", decel_lim_x_, 0.0);
-
-    traj_.get()->getNodeHandle().param<double>("max_vel_y", max_vel_y_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("min_vel_y", min_vel_y_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("acc_lim_y", acc_lim_y_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("decel_lim_y", decel_lim_y_, 0.0);
-
-    traj_.get()->getNodeHandle().param<double>("max_vel_theta", max_vel_theta_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("min_vel_theta", min_vel_theta_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
-    traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
-  }
-}
-
 void mkt_algorithm::diff::RotateToGoal::reset()
 {
+  if (initialized_)
+  {
   this->clear();
+    x_direction_ = y_direction_ = theta_direction_ = 0;
+  }
 }
 
 bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
@@ -96,13 +42,17 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DS
     robot::log_error("This planner has not been initialized, please call initialize() before using this planner");
     return false;
   }
+  if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
+  {
+    robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
+    return false;
+  }
   this->getParams();
-  global_plan_ = global_plan;
-
+  frame_id_path_ = global_plan.header.frame_id;
   goal_ = goal;
+
   double angle = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
   x_direction = y_direction = theta_direction = sign(angle);
-  ;
   return true;
 }
 

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

@@ -1,183 +0,0 @@
-#include <mkt_algorithm/diff/pure_pursuit.h>
-
-void mkt_algorithm::diff::PurePursuit::computePurePursuit(
-                                                          const score_algorithm::TrajectoryGenerator::Ptr &traj,
-                                                          const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
-                                                          const robot_nav_2d_msgs::Twist2D &velocity,
-                                                          const double &min_approach_linear_velocity,
-                                                          const double &journey_plan,
-                                                          const double &sign_x,
-                                                          const double &lookahead_dist_min,
-                                                          const double &lookahead_dist_max,
-                                                          const double &lookahead_dist,
-                                                          const double &lookahead_time,
-                                                          const double &dt,
-                                                          robot_nav_2d_msgs::Twist2D &drive_cmd)
-{
-  if (!traj)
-    return;
-  const double velocity_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x;
-  const double vel_x_reduce = std::min(fabs(velocity_max), 0.3);
-  double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
-  carrot_dist2 = std::max(carrot_dist2, 0.05);
-  double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
-
-  if (max_lateral_accel_ > 1e-6)
-  {
-    const double curvature_abs = std::max(fabs(curvature), 1e-6);
-    const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs);
-    drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit);
-  }
-
-
-  double post_cost = 0.0; 
-  double distance_error = 0.0;
-  robot_nav_2d_msgs::Twist2D twist = traj->getTwist();
-  this->applyConstraints(distance_error, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
-
-
-  double d_reduce = std::clamp(journey_plan, lookahead_dist_min, lookahead_dist_max);
-
-  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);
-  double vel_reduce = sign_x > 0
-                          ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
-                          : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
-  drive_cmd.x = (journey_plan) >= d_reduce ? drive_cmd.x : vel_reduce;
-
-  double v_theta = drive_cmd.x * curvature;
-  double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-
-  carrot_dist2 *= 0.6;
-  curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
-  v_theta = drive_cmd.x * curvature;
-
-  double limit_acc_theta = fabs(v_theta) > 0.15 ? 1.0 : 1.8;
-  double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
-  double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
-  drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
-}
-
-void mkt_algorithm::diff::PurePursuit::applyConstraints(
-  const double &dist_error, const double &lookahead_dist,
-  const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
-  const double &pose_cost, double &linear_vel, const double &sign_x)
-{
-  double curvature_vel = linear_vel;
-  double cost_vel = linear_vel;
-  double approach_vel = linear_vel;
-
-
-  if (use_regulated_linear_velocity_scaling_)
-  {
-    const double &min_rad = regulated_linear_scaling_min_radius_;
-    const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad;
-    if (radius < min_rad)
-    {
-      curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad);
-      robot_nav_2d_msgs::Twist2D cmd, result;
-      cmd.x = curvature_vel;
-      this->moveWithAccLimits(velocity, cmd, result);
-      curvature_vel = result.x;
-      linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_);
-    }
-  }
-
-  if (use_cost_regulated_linear_velocity_scaling_ &&
-      pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
-      pose_cost != static_cast<double>(robot_costmap_2d::FREE_SPACE))
-  {
-    const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
-    const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
-                                                std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
-                                            inscribed_radius;
-
-    if (min_distance_to_obstacle < cost_scaling_dist_)
-    {
-      cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_;
-    }
-    robot_nav_2d_msgs::Twist2D cmd, result;
-    cmd.x = cost_vel;
-    this->moveWithAccLimits(velocity, cmd, result);
-    cost_vel = result.x;
-    linear_vel = std::min(cost_vel, curvature_vel);
-  }
-  // ss << linear_vel << " ";
-  // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed
-  // if the actual lookahead distance is shorter than requested, that means we're at the
-  // end of the path. We'll scale linear velocity by error to slow to a smooth stop.
-  // This expression is eq. to
-  //      (1) holding time to goal, t, constant using the theoretical
-  //          lookahead distance and proposed velocity and
-  //      (2) using t with the actual lookahead
-  //          distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t).
-
-  double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr
-                                ? 2.0 * costmap_robot_->getCostmap()->getResolution()
-                                : 0.1;
-  if (dist_error > dist_error_limit)
-  {
-    double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0;
-    double unbounded_vel = approach_vel * velocity_scaling;
-    if (unbounded_vel < min_approach_linear_velocity_)
-    {
-      approach_vel = min_approach_linear_velocity_;
-    }
-    else
-    {
-      approach_vel *= velocity_scaling;
-    }
-
-    // Use the lowest velocity between approach and other constraints, if all overlapping
-    robot_nav_2d_msgs::Twist2D cmd, result;
-    cmd.x = approach_vel;
-    this->moveWithAccLimits(velocity, cmd, result);
-    approach_vel = result.x;
-    linear_vel = std::min(linear_vel, approach_vel);
-  }
-
-  // Limit linear velocities to be valid
-  double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x);
-  double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x);
-  double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y);
-  double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y);
-  double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y);
-  double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y);
-  double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel);
-  linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel);
-  linear_vel = sign_x * linear_vel;
-}
-
-double mkt_algorithm::diff::PurePursuit::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
-  double journey_plan)
-{
-double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x +
-carrot_pose.pose.y * carrot_pose.pose.y);
-
-// Avoid division by zero and handle backward motion
-if (carrot_distance < 1e-3)
-return journey_plan;
-
-// Project remaining path onto carrot direction
-double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle)
-return journey_plan * std::max(0.0, alignment);                // Only positive projection
-}
-
-
-double mkt_algorithm::diff::PurePursuit::computeDecelerationFactor(double remaining_distance, double decel_distance)
-{
-  if (remaining_distance >= decel_distance)
-  {
-    return 1.0; // Full speed
-  }
-
-  // Smooth transition using cosine function
-  double ratio = fabs(remaining_distance / decel_distance);
-  return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0
-}

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/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -120,7 +120,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        // exit(1);
+        exit(1);
       }
       rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@@ -128,11 +128,12 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      // exit(1);
     }
 
     std::string goal_checker_name;
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
+    robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
     try
     {
       robot::PluginLoaderHelper loader;
@@ -153,6 +154,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
       exit(1);
     }
+
     this->initializeOthers();
     robot::log_info("[%s:%d]\n %s is sucessed", __FILE__, __LINE__, name.c_str());
     initialized_ = true;
@@ -181,10 +183,10 @@ void pnkx_local_planner::PNKXLocalPlanner::reset()
 {
   robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
   this->unlock();
-  traj_generator_->reset();
-  goal_checker_->reset();
-  nav_algorithm_->reset();
-  rotate_algorithm_->reset();
+  if(traj_generator_) traj_generator_->reset();
+  if(goal_checker_) goal_checker_->reset();
+  if(nav_algorithm_) nav_algorithm_->reset();
+  if(rotate_algorithm_) rotate_algorithm_->reset();
   ret_nav_ = ret_angle_ = false;
 }
 
@@ -214,8 +216,6 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa
   {
     return;
   }
-  // robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(local_plan_.poses.front());
-  // pnkx_local_planner::transformGlobalPlan(tf_, local_plan_, local_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, path);
   path = local_plan_;
 
 }
@@ -250,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
   double x_direction, y_direction, theta_direction;
   if (!ret_nav_)
   {
-    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
+    if (nav_algorithm_ && !nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -258,7 +258,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
   }
   else
   {
-    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
+    if (rotate_algorithm_ && !rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -269,7 +269,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
   xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0;
   xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0;
   xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0;
-  traj_generator_->setDirect(xytheta_direct);
+  if(traj_generator_) traj_generator_->setDirect(xytheta_direct);
 }
 
 robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
@@ -307,14 +307,14 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
   }
   else if (!ret_nav_)
   {
-    traj = nav_algorithm_->calculator(pose, velocity);
+    if(nav_algorithm_) traj = nav_algorithm_->calculator(pose, velocity);
     local_plan_.header.stamp = robot::Time::now();
     robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
     local_plan_ = robot_nav_2d_utils::pathToPath(path);
   }
   else
   {
-    traj = rotate_algorithm_->calculator(pose, velocity);
+    if(rotate_algorithm_) traj = rotate_algorithm_->calculator(pose, velocity);
     local_plan_.header.stamp = robot::Time::now();
     robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
     local_plan_ = robot_nav_2d_utils::pathToPath(path);

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);