update thuat toan moi

config/costmap_common_params.yaml

@@ -1,3 +1,10 @@
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+base_local_planner: LocalPlannerAdapter
+base_global_planner: CustomPlanner
+
 robot_base_frame: base_footprint
 transform_tolerance: 1.0
 obstacle_range: 3.0

config/custom_global_params.yaml

@@ -1,4 +1,3 @@
-base_global_planner: CustomPlanner
 CustomPlanner:
   library_path: libcustom_planner
   environment_type: XYThetaLattice

config/pnkx_local_planner_params.yaml

@@ -1,10 +1,3 @@
-
-position_planner_name: PNKXLocalPlanner
-docking_planner_name: PNKXDockingLocalPlanner
-go_straight_planner_name: PNKXGoStraightLocalPlanner
-rotate_planner_name: PNKXRotateLocalPlanner
-
-base_local_planner: LocalPlannerAdapter
 LocalPlannerAdapter:
   library_path: liblocal_planner_adapter
   yaw_goal_tolerance: 0.017  
@@ -83,7 +76,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
   avoid_obstacles: false
   xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
+  angle_threshold: 0.4
   index_samples: 60
   follow_step_path: true
 
@@ -92,11 +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: 1.0                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  min_lookahead_dist: 0.4                     # 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.6                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  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) 
   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)
 
   # 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)
@@ -120,6 +114,20 @@ 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
@@ -133,9 +141,9 @@ MKTAlgorithmDiffGoStraight:
   # 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.8                    # The minimum lookahead distance (m) threshold. (default: 0.3)
+  min_lookahead_dist: 0.325                    # The minimum lookahead distance (m) threshold. (default: 0.3)
   max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 2.5                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  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) 
   max_journey_squared: 0.6                    # Minimum squared journey to consider for goal (default: 0.2) 
 

config/two_points_global_params.yaml

@@ -1,4 +1,3 @@
-base_global_planner: TwoPointsPlanner
 TwoPointsPlanner:
   library_path: libtwo_points_planner
   lethal_obstacle: 20

examples/CSharpExample.cs

@@ -53,6 +53,14 @@ namespace NavigationExample
             public double theta;
         }
 
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Twist2D
+        {
+            public double x;
+            public double y;
+            public double theta;
+        }
+
         [StructLayout(LayoutKind.Sequential)]
         public struct Quaternion
         {
@@ -93,6 +101,13 @@ namespace NavigationExample
             public Pose pose;
         }
 
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Twist2DStamped
+        {
+            public Header header;
+            public Twist2D velocity;
+        }
+
         [StructLayout(LayoutKind.Sequential)]
         public struct Vector3
         {
@@ -180,6 +195,14 @@ namespace NavigationExample
         public static extern bool navigation_set_robot_footprint(
             IntPtr handle, Point[] points, UIntPtr point_count);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_footprint(
+            IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void navigation_free_points(IntPtr points);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_to(
@@ -231,6 +254,11 @@ namespace NavigationExample
         public static extern bool navigation_get_robot_pose_2d(
             IntPtr handle, ref Pose2D out_pose);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_twist(
+            IntPtr handle, ref Twist2DStamped out_twist);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_get_feedback(

examples/NavigationExample/Program.cs

@@ -53,6 +53,14 @@ namespace NavigationExample
             public double theta;
         }
 
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Twist2D
+        {
+            public double x;
+            public double y;
+            public double theta;
+        }
+
         [StructLayout(LayoutKind.Sequential)]
         public struct Quaternion
         {
@@ -93,6 +101,13 @@ namespace NavigationExample
             public Pose pose;
         }
 
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Twist2DStamped
+        {
+            public Header header;
+            public Twist2D velocity;
+        }
+
         [StructLayout(LayoutKind.Sequential)]
         public struct Vector3
         {
@@ -180,6 +195,14 @@ namespace NavigationExample
         public static extern bool navigation_set_robot_footprint(
             IntPtr handle, Point[] points, UIntPtr point_count);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_footprint(
+            IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void navigation_free_points(IntPtr points);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_to(
@@ -231,6 +254,11 @@ namespace NavigationExample
         public static extern bool navigation_get_robot_pose_2d(
             IntPtr handle, ref Pose2D out_pose);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_twist(
+            IntPtr handle, ref Twist2DStamped out_twist);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_get_feedback(

src/APIs/c_api/include/nav_c_api.h

@@ -59,6 +59,15 @@ typedef struct {
     double theta;
 } Pose2D;
 
+/**
+ * @brief Twist2D structure (x, y, theta velocities)
+ */
+typedef struct {
+    double x;
+    double y;
+    double theta;
+} Twist2D;
+
 /**
  * @brief Quaternion structure
  */
@@ -104,6 +113,14 @@ typedef struct {
     Pose pose;
 } PoseStamped;
 
+/**
+ * @brief Twist2DStamped structure
+ */
+typedef struct {
+    Header header;
+    Twist2D velocity;
+} Twist2DStamped;
+
 /**
  * @brief Vector3 structure
  */
@@ -250,6 +267,21 @@ bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
  */
 bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count);
 
+/**
+ * @brief Get the robot's footprint (outline shape)
+ * @param handle Navigation handle
+ * @param out_points Output array of points (allocated by library, free with navigation_free_points)
+ * @param out_count Output number of points in the array
+ * @return true on success, false on failure
+ */
+bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count);
+
+/**
+ * @brief Free a points array allocated by navigation_get_robot_footprint
+ * @param points Pointer to point array
+ */
+void navigation_free_points(Point* points);
+
 /**
  * @brief Send a goal for the robot to navigate to
  * @param handle Navigation handle
@@ -350,6 +382,15 @@ bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out
  */
 bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
 
+/**
+ * @brief Get the robot's current twist
+ * @param handle Navigation handle
+ * @param out_twist Output parameter with the robot's current twist
+ * @return true if twist was successfully retrieved
+ * @note out_twist->header.frame_id must be freed using nav_c_api_free_string
+ */
+bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist);
+
 /**
  * @brief Get navigation feedback
  * @param handle Navigation handle

src/APIs/c_api/src/nav_c_api.cpp

@@ -6,12 +6,14 @@
 #include <robot_geometry_msgs/Pose2D.h>
 #include <robot_geometry_msgs/Point.h>
 #include <robot_geometry_msgs/Vector3.h>
+#include <robot_std_msgs/Header.h>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
 #include <tf3/buffer_core.h>
 #include <string>
 #include <vector>
 #include <cstring>
 #include <cstdlib>
-#include <boost/dll/import.hpp>
+#include <stdexcept>
 #include <robot/robot.h>
 
 
@@ -65,6 +67,25 @@ namespace {
         c_pose->theta = cpp_pose.theta;
     }
 
+    void cpp_to_c_header(const robot_std_msgs::Header& cpp_header, Header* c_header) {
+        c_header->seq = cpp_header.seq;
+        c_header->sec = cpp_header.stamp.sec;
+        c_header->nsec = cpp_header.stamp.nsec;
+        if (!cpp_header.frame_id.empty()) {
+            c_header->frame_id = strdup(cpp_header.frame_id.c_str());
+        } else {
+            c_header->frame_id = nullptr;
+        }
+    }
+
+    void cpp_to_c_twist2d_stamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist,
+                                  Twist2DStamped* c_twist) {
+        cpp_to_c_header(cpp_twist.header, &c_twist->header);
+        c_twist->velocity.x = cpp_twist.velocity.x;
+        c_twist->velocity.y = cpp_twist.velocity.y;
+        c_twist->velocity.theta = cpp_twist.velocity.theta;
+    }
+
     // Convert C++ NavFeedback to C NavFeedback
     void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
         c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
@@ -226,22 +247,13 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
     }
     try {
         printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
-        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
-        robot::PluginLoaderHelper loader;
+        if (!tf_ptr || !(*tf_ptr)) {
-        std::string lib_path = loader.findLibraryPath("MoveBase");
+            printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__);
-        auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
-            lib_path,
-            "MoveBase", 
-            boost::dll::load_mode::append_decorations);
-            
-        robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
-        printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__);
-        if (nav_ptr == nullptr) {
-            printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__);
             return false;
         }
-        nav_ptr->initialize(*tf_ptr);
+        nav->initialize(*tf_ptr);
     
         return true;    
     } catch (const std::exception &e) {
@@ -276,6 +288,45 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
     }
 }
 
+extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count) {
+    if (!handle || !out_points || !out_count) {
+        return false;
+    }
+
+    try {
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        std::vector<robot_geometry_msgs::Point> footprint = nav->getRobotFootprint();
+        if (footprint.empty()) {
+            *out_points = nullptr;
+            *out_count = 0;
+            return true;
+        }
+
+        Point* points = static_cast<Point*>(malloc(sizeof(Point) * footprint.size()));
+        if (!points) {
+            return false;
+        }
+
+        for (size_t i = 0; i < footprint.size(); ++i) {
+            points[i].x = footprint[i].x;
+            points[i].y = footprint[i].y;
+            points[i].z = footprint[i].z;
+        }
+
+        *out_points = points;
+        *out_count = footprint.size();
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" void navigation_free_points(Point* points) {
+    if (points) {
+        free(points);
+    }
+}
+
 extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
                                     double xy_goal_tolerance, double yaw_goal_tolerance) {
     if (!handle || !goal) {
@@ -442,6 +493,21 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
     }
 }
 
+extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist) {
+    if (!handle || !out_twist) {
+        return false;
+    }
+
+    try {
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist();
+        cpp_to_c_twist2d_stamped(cpp_twist, out_twist);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
 extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) {
     if (!handle || !out_feedback) {
         return false;

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

@@ -153,11 +153,6 @@ namespace score_algorithm
     double old_xy_goal_tolerance_;
     double angle_threshold_;
 
-    bool enable_publish_;
-    // robot::Publisher reached_intermediate_goals_pub_;
-    // robot::Publisher current_goal_pub_;
-    // robot::Publisher closet_robot_goal_pub_;
-    // robot::Publisher transformed_plan_pub_, compute_plan_pub_;
 
     std::vector<robot_nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
     std::vector<unsigned int> start_index_saved_vt_;

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

@@ -98,6 +98,12 @@ namespace score_algorithm
      */
     virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0;
 
+    /**
+     * @brief Get the current twist
+     * @return The current twist
+     */
+    virtual robot_nav_2d_msgs::Twist2D getTwist() = 0;
+
     /**
      * @brief Get all the twists for an iteration.
      *

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -81,9 +81,10 @@ 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

@@ -3,8 +3,6 @@
 
 #include <robot/robot.h>
 #include <score_algorithm/score_algorithm.h>
-
-#include <boost/dll/alias.hpp>
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_geometry_msgs/PointStamped.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
@@ -12,8 +10,6 @@
 #include <robot_nav_core2/costmap.h>
 #include <nav_grid/coordinate_conversion.h>
 #include <angles/angles.h>
-#include <tf3/exceptions.h>
-#include <tf3/utils.h>
 #include <robot_nav_msgs/Path.h>
 #include <kalman/kalman.h>
 #include <vector>
@@ -44,7 +40,7 @@ namespace mkt_algorithm
        * @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;
+        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
@@ -101,11 +97,6 @@ namespace mkt_algorithm
        */
       virtual void getParams();
 
-      /**
-       * @brief Initialize Kalman filter
-       */
-      virtual void initKalmanFilter();
-
       /**
        * @brief Dynamically adjust look ahead distance based on the speed
        * @param velocity
@@ -237,7 +228,7 @@ namespace mkt_algorithm
       size_t window_size_;
 
       bool initialized_;
-      bool nav_stop_, avoid_obstacles_;
+      bool nav_stop_;
       robot::NodeHandle nh_, nh_priv_;
       std::string frame_id_path_;
       std::string robot_base_frame_;

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

@@ -0,0 +1,314 @@
+#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/pure_pursuit.h

@@ -0,0 +1,52 @@
+#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

@@ -0,0 +1,121 @@
+## Hermite (quỹ đạo 2D) từ robot đến look-ahead pose
+
+![Hermite trajectory](hermite_trajectory.svg)
+
+### Bài toán
+Cho pose đích trong hệ tọa độ robot: `goal = (x, y, theta)`, với robot ở gốc
+`(0, 0, 0)` (hướng theo trục +x). Cần sinh quỹ đạo từ robot đến pose đích.
+
+### Ý tưởng
+Dùng đường cong Hermite bậc 3 để đảm bảo:
+- Đi qua đúng điểm đầu và cuối.
+- Đúng hướng đầu và cuối.
+
+Đường cong tham số `t` từ `0` đến `1`:
+
+```
+p(t) = (x(t), y(t))
+```
+
+Ràng buộc:
+- p(0) = (0, 0), hướng 0 rad.
+- p(1) = (x, y), hướng theta rad.
+
+### Hệ số Hermite
+```
+h00 =  2t^3 - 3t^2 + 1
+h10 =  t^3 - 2t^2 + t
+h01 = -2t^3 + 3t^2
+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)):
+```
+p'(0) = (L, 0)
+p'(1) = (L*cos(theta), 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)
+```
+
+### Hướng (heading) trên đường cong
+Đạo hàm:
+```
+h00' = 6t^2 - 6t
+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)
+```
+Hướng:
+```
+heading(t) = atan2(dy, dx)
+```
+
+### Cách sử dụng
+Lấy mẫu `t` đều (ví dụ 50-200 điểm) để tạo danh sách điểm quỹ đạo.
+Nếu cần độ cong, có thể tính thêm từ đạo hàm bậc 2.
+
+### Ví dụ code C++
+```
+#include <vector>
+#include <cmath>
+
+struct Pose2D {
+  double x;
+  double y;
+  double theta;
+};
+
+std::vector<Pose2D> generateHermiteTrajectory(
+    double x, double y, double theta,
+    int samples = 100)
+{
+  std::vector<Pose2D> path;
+  path.reserve(samples + 1);
+
+  double L = std::sqrt(x * x + y * y);
+  if (L < 1e-6) {
+    path.push_back({0.0, 0.0, 0.0});
+    return path;
+  }
+
+  for (int i = 0; i <= samples; ++i) {
+    double t = static_cast<double>(i) / samples;
+    double t2 = t * t;
+    double t3 = t2 * t;
+
+    double h00 =  2 * t3 - 3 * t2 + 1;
+    double h10 =      t3 - 2 * t2 + t;
+    double h01 = -2 * t3 + 3 * t2;
+    double h11 =      t3 -     t2;
+
+    double px = h10 * L + h01 * x + h11 * (L * std::cos(theta));
+    double py = h01 * y + h11 * (L * std::sin(theta));
+
+    double dh00 = 6 * t2 - 6 * t;
+    double dh10 = 3 * t2 - 4 * t + 1;
+    double dh01 = -6 * t2 + 6 * t;
+    double dh11 = 3 * t2 - 2 * t;
+
+    double dx = dh10 * L + dh01 * x + dh11 * (L * std::cos(theta));
+    double dy = dh01 * y + dh11 * (L * std::sin(theta));
+
+    double heading = std::atan2(dy, dx);
+
+    path.push_back({px, py, heading});
+  }
+
+  return path;
+}
+```
+
+### Ghi chú
+- Nếu (x, y) rất gần 0 thì quỹ đạo suy biến, chỉ trả về điểm gốc.
+- L có thể điều chỉnh lớn hơn để quỹ đạo mềm hơn.

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

@@ -15,7 +15,6 @@ void mkt_algorithm::diff::GoStraight::initialize(
     costmap_robot_ = costmap_robot;
 
     this->getParams();
-    nh_.param("publish_topic", enable_publish_, false);
     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>();
@@ -107,7 +106,7 @@ 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 (traj_->hasMoreTwists())
+  while (robot::ok() && traj_->hasMoreTwists())
   {
     twist = traj_->nextTwist();
     // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
@@ -184,6 +183,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   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_;

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

@@ -1,25 +1,23 @@
 #include <mkt_algorithm/diff/diff_predictive_trajectory.h>
-#include <boost/dll/import.hpp>
+#include <boost/dll/alias.hpp>
-#include <robot/robot.h>
-
 #define LIMIT_VEL_THETA 0.33
 
+mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}
+
 void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
-    nh_ = robot::NodeHandle("~");
+    nh_ = robot::NodeHandle("~/");
     nh_priv_ = robot::NodeHandle(nh, name);
     name_ = name;
     tf_ = tf;
     traj_ = traj;
     costmap_robot_ = costmap_robot;
     this->getParams();
-    nh_.param("publish_topic", enable_publish_, false);
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
-    footprint_spec_ = costmap_robot_->getRobotFootprint();
     std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
     if (footprint.size() > 1)
     {
@@ -42,65 +40,55 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
       this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
       this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
     }
-    this->initKalmanFilter();
+
+    // kalman
+    last_actuator_update_ = robot::Time::now();
+    n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
+    m_ = 2; // measurements: x, y, theta
+    double dt = control_duration_;
+
+    // Khởi tạo ma trận
+    A = Eigen::MatrixXd::Identity(n_, n_);
+    C = Eigen::MatrixXd::Zero(m_, n_);
+    Q = Eigen::MatrixXd::Zero(n_, n_);
+    R = Eigen::MatrixXd::Identity(m_, m_);
+    P = Eigen::MatrixXd::Identity(n_, n_);
+
+    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;
+    }
+
+    C(0, 0) = 1;
+    C(1, 3) = 1;
+    Q(2, 2) = 0.1;
+    Q(5, 5) = 0.6;
+
+    R(0, 0) = 0.1;
+    R(1, 1) = 0.2;
+
+    P(3, 3) = 0.4;
+    P(4, 4) = 0.4;
+    P(5, 5) = 0.4;
+
+    kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
+    Eigen::VectorXd x0(n_);
+    x0 << 0, 0, 0, 0, 0, 0;
+    kf_->init(robot::Time::now().toSec(), x0);
+
+
+
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->initialized_ = true;
     robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__);
   }
 }
 
-mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory()
-{
-  if (kf_)
-  {
-    kf_.reset();
-  }
-}
-
-void  mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter()
-{
-  // kalman
-  last_actuator_update_ = robot::Time::now();
-  n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
-  m_ = 2; // measurements: x, y, theta
-  double dt = control_duration_;
-
-  // Khởi tạo ma trận
-  A = Eigen::MatrixXd::Identity(n_, n_);
-  C = Eigen::MatrixXd::Zero(m_, n_);
-  Q = Eigen::MatrixXd::Zero(n_, n_);
-  R = Eigen::MatrixXd::Identity(m_, m_);
-  P = Eigen::MatrixXd::Identity(n_, n_);
-
-  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;
-  }
-
-  C(0, 0) = 1;
-  C(1, 3) = 1;
-  Q(2, 2) = 0.1;
-  Q(5, 5) = 0.6;
-
-  R(0, 0) = 0.1;
-  R(1, 1) = 0.2;
-
-  P(3, 3) = 0.4;
-  P(4, 4) = 0.4;
-  P(5, 5) = 0.4;
-
-  kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
-  Eigen::VectorXd x0(n_);
-  x0 << 0, 0, 0, 0, 0, 0;
-  kf_->init(robot::Time::now().toSec(), x0);
-}
-
 void mkt_algorithm::diff::PredictiveTrajectory::getParams()
 {
-  robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
+  robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
-  nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
   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);
@@ -134,8 +122,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   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, "
+    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__);
-             "it should be >0. Disabling cost regulated linear velocity scaling.");
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
   double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
@@ -297,10 +284,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
     return false;
   }
-  // else
+
-  // {
-  //   robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
-  // }
 
   x_direction = x_direction_;
   y_direction = y_direction_ = 0;
@@ -354,7 +338,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       // teb_local_planner::PoseSE2 goal_pose(back);
       // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
       const double dir_path = 0.0;
-
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
     }
@@ -392,19 +375,30 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   double sign_x = sign(x_direction_);
   robot_nav_2d_msgs::Twist2D twist;
   traj_->startNewIteration(velocity);
-  while (traj_->hasMoreTwists())
+  while (robot::ok() && traj_->hasMoreTwists())
   {
     twist = traj_->nextTwist();
   }
   drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
     robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
+    stopWithAccLimits(pose, velocity, drive_cmd);
+    result.velocity = drive_cmd;
+    prevous_drive_cmd_ = drive_cmd;
     return result;
   }
+  else
+  {
+    result.poses.clear();
+    result.poses.reserve(transformed_plan.poses.size());
+    for (const auto &pose_stamped : transformed_plan.poses)
+    {
+      result.poses.push_back(pose_stamped.pose);
+    }
+  }
 
   double lookahead_dist = getLookAheadDistance(velocity);
   double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y);
@@ -415,21 +409,16 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     return result;
   }
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
+
   bool allow_rotate = false;
   nh_priv_.param("allow_rotate", allow_rotate, false);
 
-  // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y);
   robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
   const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
-  if (avoid_obstacles_)
-    allow_rotate = journey_plan >= distance_allow_rotate &&
-                   fabs(front.y) <= 0.2 &&
-                   (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution());
-  else
-    allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
 
+  allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
   double angle_to_heading;
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
   {
@@ -515,18 +504,18 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
     drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
 
-      if (this->nav_stop_)
+    if (this->nav_stop_)
+    {
+      if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
       {
-        if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
+        if (!stopWithAccLimits(pose, velocity, drive_cmd))
-        {
+          return result;
-          if (!stopWithAccLimits(pose, velocity, drive_cmd))
-            return result;
-        }
-        else
-          drive_cmd = {};
-        result.velocity = drive_cmd;
-        return result;
       }
+      else
+        drive_cmd = {};
+      result.velocity = drive_cmd;
+      return result;
+    }
 
     Eigen::VectorXd y(2);
     y << drive_cmd.x, drive_cmd.theta;
@@ -543,19 +532,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     }
     kf_->update(y, dt, A);
 
-    // Check if Kalman filter's estimated velocity exceeds v_max
+    drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
-    {
-      drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
-    }
-    else
-    {
-      drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(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 = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA);
   }
@@ -790,7 +767,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
     robot_nav_2d_msgs::Pose2DStamped robot;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot))
     {
-      throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
     }
 
     double dist_thresh_sq = dist_behind_robot * dist_behind_robot;
@@ -818,7 +795,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
   }
   catch (const tf3::TransformException &ex)
   {
-    robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what());
+    robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s\n", __FILE__, __LINE__, ex.what());
     return false;
   }
   return true;
@@ -855,7 +832,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
     robot_nav_2d_msgs::Pose2DStamped robot_pose;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
     {
-      throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
     }
 
     // we'll discard points on the plan that are outside the local costmap
@@ -934,19 +911,19 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
   }
   catch (tf3::LookupException &ex)
   {
-    robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n No Transform available Error: %s\n", __FILE__, __LINE__, ex.what());
     return false;
   }
   catch (tf3::ConnectivityException &ex)
   {
-    robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n Connectivity Error: %s\n", __FILE__, __LINE__, ex.what());
     return false;
   }
   catch (tf3::ExtrapolationException &ex)
   {
-    robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n Extrapolation Error: %s\n", __FILE__, __LINE__, ex.what());
     if (global_plan.poses.size() > 0)
-      robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
+      robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
 
     return false;
   }
@@ -993,7 +970,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_na
   double vel_yaw = velocity.theta;
   double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt));
 
-  // we do want to check whether or not the command is valid
   cmd_vel.x = vx;
   cmd_vel.y = vy;
   cmd_vel.theta = vth;
@@ -1217,94 +1193,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
   return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0;
 }
 
-void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose)
-{
-  const auto &plan_back_pose = compute_plan_.poses.back();
-  // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0;
-  // const double offset_min = this->min_path_distance_;
-
-  auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-  for (const auto &point : points_rb)
-  {
-    double cost_goal = costAtPose(point.x, point.y);
-    double dx = point.x - pose.pose.x;
-    double dy = point.y - pose.pose.y;
-    double cos_yaw = cos(-pose.pose.theta);
-    double sin_yaw = sin(-pose.pose.theta);
-    double y_rel = dx * sin_yaw + dy * cos_yaw;
-    const double epsilon = min_path_distance_;
-    if (y_rel > epsilon)
-    {
-      cost_left_side_ = std::max(cost_left_side_, cost_goal);
-    }
-    else if (y_rel < -epsilon)
-    {
-      cost_right_side_ = std::max(cost_right_side_, cost_goal);
-    }
-  }
-
-  unsigned int step_footprint = 10;
-  if ((unsigned int)(compute_plan_.poses.size() - 1) < 10)
-  {
-    auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-    for (const auto &point : points)
-    {
-      double cost_goal = costAtPose(point.x, point.y);
-      double dx = point.x - plan_back_pose.pose.x;
-      double dy = point.y - plan_back_pose.pose.y;
-      double cos_yaw = cos(-plan_back_pose.pose.theta);
-      double sin_yaw = sin(-plan_back_pose.pose.theta);
-      double y_rel = dx * sin_yaw + dy * cos_yaw;
-      const double epsilon = min_path_distance_;
-      if (y_rel > epsilon)
-      {
-        cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
-      }
-      else if (y_rel < -epsilon)
-      {
-        cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
-      }
-      else
-        center_cost_ = std::max(center_cost_, cost_goal);
-    }
-  }
-  else
-  {
-    for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint)
-    {
-      auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-      for (const auto &point : points)
-      {
-        double cost_goal = costAtPose(point.x, point.y);
-        double dx = point.x - compute_plan_.poses[i].pose.x;
-        double dy = point.y - compute_plan_.poses[i].pose.y;
-        double cos_yaw = cos(-compute_plan_.poses[i].pose.theta);
-        double sin_yaw = sin(-compute_plan_.poses[i].pose.theta);
-        double y_rel = dx * sin_yaw + dy * cos_yaw;
-        const double epsilon = min_path_distance_;
-        if (y_rel > epsilon)
-        {
-          cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
-        }
-        else if (y_rel < -epsilon)
-        {
-          cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
-        }
-        else
-          center_cost_ = std::max(center_cost_, cost_goal);
-      }
-      double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x;
-      double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y;
-      if (hypot(dx, dy) > 1.0)
-        break;
-    }
-  }
-}
-
 score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
 {
   return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();
 }
 
-// Register this planner as a GlobalPlanner plugin
 BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory)

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

@@ -25,7 +25,6 @@ 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<bool>("avoid_obstacles", avoid_obstacles_, false);
   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);

src/Algorithms/Libraries/mkt_algorithm/src/diff/hermite_trajectory.svg

@@ -0,0 +1,40 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="720" height="420" viewBox="0 0 720 420">
+  <rect width="100%" height="100%" fill="white"/>
+  <g stroke="#444" stroke-width="2" fill="none">
+    <line x1="80" y1="340" x2="660" y2="340"/>
+    <line x1="80" y1="340" x2="80" y2="60"/>
+    <polygon points="660,340 648,334 648,346" fill="#444"/>
+    <polygon points="80,60 74,72 86,72" fill="#444"/>
+  </g>
+
+  <g font-family="Arial, sans-serif" font-size="14" fill="#222">
+    <text x="668" y="345">x</text>
+    <text x="68" y="52">y</text>
+    <text x="92" y="356">O(0,0)</text>
+  </g>
+
+  <g stroke="#1f77b4" stroke-width="3" fill="none">
+    <!-- Hermite-like curve -->
+    <path d="M 80 340 C 220 310, 340 240, 460 180 S 600 110, 640 120"/>
+  </g>
+
+  <g fill="#1f77b4">
+    <circle cx="80" cy="340" r="5"/>
+    <circle cx="640" cy="120" r="5"/>
+  </g>
+
+  <g stroke="#888" stroke-width="2" fill="none">
+    <!-- Start heading (0 rad) -->
+    <line x1="80" y1="340" x2="140" y2="340"/>
+    <polygon points="140,340 130,334 130,346" fill="#888"/>
+    <!-- Goal heading (theta) - aligned with curve tangent -->
+    <line x1="640" y1="120" x2="690" y2="130"/>
+    <polygon points="690,130 678,124 678,136" fill="#888"/>
+  </g>
+
+  <g font-family="Arial, sans-serif" font-size="14" fill="#222">
+    <text x="150" y="334">heading 0</text>
+    <text x="648" y="150">goal (x,y,theta)</text>
+    <text x="470" y="210">Hermite trajectory</text>
+  </g>
+</svg>

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

@@ -0,0 +1,183 @@
+#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/include/mkt_plugins/kinematic_parameters.h

@@ -270,6 +270,23 @@ namespace mkt_plugins
     using Ptr = std::shared_ptr<KinematicParameters>;
 
   protected:
+    
+    /**
+     * @brief Configure the kinematic parameters
+     * @param nh NodeHandle
+     *
+     * Configures the kinematic parameters from the given NodeHandle.
+     */
+    void configure(const robot::NodeHandle &nh);
+
+    /**
+     * @brief Reconfigure the kinematic parameters
+     * @param nh NodeHandle
+     *
+     * Reconfigures the kinematic parameters from the given NodeHandle.
+     */
+    void reconfigure(const robot::NodeHandle &nh);
+  
     // For parameter descriptions, see cfg/KinematicParams.cfg
     int xytheta_direct_[3];
     double min_vel_x_, min_vel_y_;

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h

@@ -116,8 +116,9 @@ namespace mkt_plugins
      * current velocity within acc_time seconds, respecting acceleration/deceleration limits.
      * The velocities range from min_vel_ to max_vel_ (projected from current velocity).
      */
-    OneDVelocityIterator(double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time,
+    OneDVelocityIterator(
-                         int num_samples)
+      std::string name, double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, int num_samples)
+     : name_(name)
     {
       // if (current < min)
       // {
@@ -203,8 +204,6 @@ namespace mkt_plugins
      */
     bool isFinished() const
     {
-      // if(name_ == std::string("th_it_"))
-      //   ROS_INFO("%s %f %f", name_.c_str(), current_, max_vel_ + EPSILON);
       double limit_vel = direct_ > 0? max_vel_ : min_vel_;
       return fabs(current_) > fabs(limit_vel) + EPSILON;
     }

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h

@@ -82,6 +82,12 @@ namespace mkt_plugins
      */
     robot_nav_2d_msgs::Twist2D nextTwist() override;
 
+    /**
+     * @brief Get the current velocity
+     * @return The current velocity
+     */
+    robot_nav_2d_msgs::Twist2D getTwist() override;
+    
     /**
      * @brief Generate a trajectory from start pose to goal
      * @param start_pose Starting pose of the robot

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h

@@ -54,6 +54,12 @@ public:
    * Should only be called when hasMoreTwists() returns true.
    */
   virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0;
+
+  /**
+   * @brief Get the current velocity
+   * @return The current velocity
+   */
+  virtual robot_nav_2d_msgs::Twist2D getTwist() = 0;
 };  // class VelocityIterator
 
 }  // namespace mkt_plugins

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h

@@ -55,6 +55,12 @@ namespace mkt_plugins
      */
     robot_nav_2d_msgs::Twist2D nextTwist() override;
 
+    /**
+     * @brief Get the current velocity
+     * @return The current velocity (x, y, theta)
+     */
+    robot_nav_2d_msgs::Twist2D getTwist() override;
+    
   protected:
     /**
      * @brief Check if the current velocity combination is valid

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

@@ -54,10 +54,48 @@ namespace mkt_plugins
     setDecelerationAsNeeded(nh, "y");
     setDecelerationAsNeeded(nh, "theta");
 
-
+    configure(nh);
     xytheta_direct_[0] = xytheta_direct_[1] = xytheta_direct_[2] = 1;
   }
 
+
+  void KinematicParameters::configure(const robot::NodeHandle &nh)
+  {
+    nh.param("max_vel_x", original_max_vel_x_, 0.0);
+    nh.param("max_vel_y", original_max_vel_y_, 0.0);
+    nh.param("max_vel_theta", original_max_vel_theta_, 0.0);
+    nh.param("min_vel_x", original_min_vel_x_, 0.0);
+    nh.param("min_vel_y", original_min_vel_y_, 0.0);
+    nh.param("min_speed_xy", original_min_speed_xy_, 0.0);
+    nh.param("max_speed_xy", original_max_speed_xy_, 0.0);
+    nh.param("min_speed_theta", original_min_speed_theta_, 0.0);
+    nh.param("acc_lim_x", original_acc_lim_x_, 0.0);
+    nh.param("acc_lim_y", original_acc_lim_y_, 0.0);
+    nh.param("acc_lim_theta", original_acc_lim_theta_, 0.0);
+    nh.param("decel_lim_x", original_decel_lim_x_, 0.0);
+    nh.param("decel_lim_y", original_decel_lim_y_, 0.0);
+    nh.param("decel_lim_theta", original_decel_lim_theta_, 0.0);
+    reconfigure(nh);
+  }
+
+  void KinematicParameters::reconfigure(const robot::NodeHandle &nh)
+  {
+    nh.param("max_vel_x", max_vel_x_, 0.0);
+    nh.param("max_vel_y", max_vel_y_, 0.0);
+    nh.param("max_vel_theta", max_vel_theta_, 0.0);
+    nh.param("min_vel_x", min_vel_x_, 0.0);
+    nh.param("min_vel_y", min_vel_y_, 0.0);
+    nh.param("min_speed_xy", min_speed_xy_, 0.0);
+    nh.param("max_speed_xy", max_speed_xy_, 0.0);
+    nh.param("min_speed_theta", min_speed_theta_, 0.0);
+    nh.param("acc_lim_x", acc_lim_x_, 0.0);
+    nh.param("acc_lim_y", acc_lim_y_, 0.0);
+    nh.param("acc_lim_theta", acc_lim_theta_, 0.0);
+    nh.param("decel_lim_x", decel_lim_x_, 0.0);
+    nh.param("decel_lim_y", decel_lim_y_, 0.0);
+    nh.param("decel_lim_theta", decel_lim_theta_, 0.0);
+  }
+
   void KinematicParameters::setDirect(int *xytheta_direct)
   {
     xytheta_direct_[0] = xytheta_direct[0];
@@ -111,13 +149,11 @@ namespace mkt_plugins
   void KinematicParameters::setMinX(double value)
   {
     min_vel_x_ = fabs(value) < fabs(original_min_vel_x_) + EPSILON ? value : min_vel_x_;
-    // ROS_INFO_THROTTLE(10.0, "vx_min %f %f %f", value , original_min_vel_x_, min_vel_x_);
   }
 
   void KinematicParameters::setMaxX(double value)
   {
     max_vel_x_ = fabs(value) <= fabs(original_max_vel_x_) + EPSILON ? value : original_max_vel_x_;
-    // ROS_INFO_THROTTLE(10, "vx %f %f %f", value , original_max_vel_x_, max_vel_x_);
   }
 
   void KinematicParameters::setAccX(double value)

src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp

@@ -18,6 +18,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
   else
   {
     double controller_frequency = robot_nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0);
+    robot::log_warning("[%s:%d] limited_accel_generator: %f", __FILE__, __LINE__, controller_frequency);
     if (controller_frequency > 0)
     {
       acceleration_time_ = 1.0 / controller_frequency;
@@ -29,6 +30,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
       acceleration_time_ = 0.05;
     }
   }
+
 }
 
 void LimitedAccelGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity)

src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp

@@ -166,6 +166,11 @@ namespace mkt_plugins
     return velocity_iterator_->nextTwist();
   }
 
+  robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::getTwist()
+  {
+    return velocity_iterator_->getTwist();
+  }
+
   std::vector<double> StandardTrajectoryGenerator::getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel)
   {
     std::vector<double> steps;

src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp

@@ -14,19 +14,14 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter
 
 void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt)
 {
-  x_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
+  x_it_ = std::make_shared<OneDVelocityIterator>("x_it_", current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
                                                  kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_);
-  x_it_->name_ = std::string("x_it_");
   
-  y_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(),
+  y_it_ = std::make_shared<OneDVelocityIterator>("y_it_", current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(),
                                                  kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_);
   
-  y_it_->name_ = std::string("y_it_");
+  th_it_ = std::make_shared<OneDVelocityIterator>("th_it_", current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
-  
-  th_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
                                                   kinematics_->getAccTheta(), kinematics_->getDecelTheta(), dt, vtheta_samples_);
-
-  th_it_->name_ = std::string("th_it_");
 }
 
 bool XYThetaIterator::hasMoreTwists()
@@ -44,6 +39,15 @@ robot_nav_2d_msgs::Twist2D XYThetaIterator::nextTwist()
   return velocity;
 }
 
+robot_nav_2d_msgs::Twist2D XYThetaIterator::getTwist()
+{
+  robot_nav_2d_msgs::Twist2D velocity;
+  velocity.x = x_it_->getVelocity();
+  velocity.y = y_it_->getVelocity();
+  velocity.theta = th_it_->getVelocity();
+  return velocity;
+}
+
 bool XYThetaIterator::isValidVelocity()
 {
   return kinematics_->isValidSpeed(x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity());

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h

@@ -46,6 +46,12 @@ namespace pnkx_local_planner
      */
     void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
 
+    /**
+     * @brief robot_nav_core2 getPlan - Gets the current global plan
+     * @param path The global plan
+     */
+    void getPlan(robot_nav_2d_msgs::Path2D &path) override;
+
     /**
      * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *
@@ -179,7 +185,8 @@ namespace pnkx_local_planner
     std::string name_;
     robot::NodeHandle parent_, planner_nh_;  
     robot_nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan
-    robot_nav_2d_msgs::Path2D transformed_plan_;
+    robot_nav_2d_msgs::Path2D transformed_global_plan_;
+    robot_nav_2d_msgs::Path2D local_plan_;
     robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
  
     robot_costmap_2d::Costmap2D *costmap_;

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp

@@ -180,7 +180,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
 
   try
   {
-    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
   }
   catch(const robot_nav_core2::LocalPlannerException& e)
@@ -189,7 +189,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
   }
 
   double x_direction, y_direction, theta_direction;
-  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
   {
     throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
   }
@@ -206,9 +206,16 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
   robot_nav_2d_msgs::Twist2D twist;
   mkt_msgs::Trajectory2D traj;
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
-  if (!ret_nav_)
+  if (ret_nav_)
-    traj = nav_algorithm_->calculator(pose, velocity);
+  {
+    local_plan_.poses.clear();
+    return cmd_vel;
+  }
 
+  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_->getGlobalFrameID(), robot::Time::now());
+  local_plan_ = robot_nav_2d_utils::pathToPath(path);
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
 }
@@ -222,7 +229,7 @@ bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const robot_n
   }
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
-  ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_plan_, velocity);
+  ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_global_plan_, velocity);
 
   if (ret_nav_)
     robot::log_info_at(__FILE__, __LINE__, "Goal reached!");

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -105,7 +105,7 @@ 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 algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      // exit(1);
     }
 
     std::string algorithm_rotate_name;
@@ -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());
@@ -208,6 +208,18 @@ void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path
   }
 }
 
+void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &path)
+{
+  if (local_plan_.poses.empty())
+  {
+    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_;
+
+}
+
 void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
   this->getParams(planner_nh_);
@@ -225,13 +237,11 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
   }
 
   // Update time stamp of goal pose
-  // goal_pose_.header.stamp = pose.header.stamp;
-  // robot::log_info("pose: %f %f %f", pose.pose.x, pose.pose.y, pose.pose.theta);
   robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
                              local_goal_pose = this->transformPoseToLocal(goal_pose_);
-  // robot::log_info("local_start_pose: %f %f %f", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
+
-  // robot::log_info("local_goal_pose: %f %f %f", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
+  if (!pnkx_local_planner::transformGlobalPlan(
-  if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
   {
     robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
     throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
@@ -240,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_plan_, x_direction, y_direction, theta_direction))
+    if (!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");
@@ -248,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_plan_, x_direction, y_direction, theta_direction))
+    if (!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");
@@ -291,11 +301,24 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
   cmd_vel.header.stamp = robot::Time::now();
 
   if (ret_nav_ && ret_angle_)
+  {
     return cmd_vel;
+    local_plan_.poses.clear();
+  }
   else if (!ret_nav_)
+  {
     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);
+    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);
+  }
 
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
@@ -404,7 +427,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const robot_nav_2d_msgs
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
   robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
-  robot_nav_2d_msgs::Path2D plan = transformed_plan_;
+  robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
   if (!ret_nav_)
   {
     ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity);

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp

@@ -155,7 +155,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
 
   try
   {
-    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
   }
   catch(const robot_nav_core2::LocalPlannerException& e)
@@ -164,7 +164,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
   }
 
   double x_direction, y_direction, theta_direction;
-  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
   {
     throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
   }
@@ -199,10 +199,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::Sc
   robot_nav_2d_msgs::Twist2D twist;
   mkt_msgs::Trajectory2D traj;
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
+  cmd_vel.header.stamp = robot::Time::now();
   if (ret_nav_)
+  {
+    local_plan_.poses.clear();
     return cmd_vel;
+  }
 
   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_->getGlobalFrameID(), robot::Time::now());
+  local_plan_ = robot_nav_2d_utils::pathToPath(path);
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
 }

src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h

@@ -159,20 +159,20 @@ protected:
    */
 		TF3SIMD_FORCE_INLINE void	setMax(const QuadWord& other)
 		{
-			// tf3SetMax(m_floats[0], other.m_floats[0]);
+			tf3SetMax(m_floats[0], other.m_floats[0]);
-			// tf3SetMax(m_floats[1], other.m_floats[1]);
+			tf3SetMax(m_floats[1], other.m_floats[1]);
-			// tf3SetMax(m_floats[2], other.m_floats[2]);
+			tf3SetMax(m_floats[2], other.m_floats[2]);
-			// tf3SetMax(m_floats[3], other.m_floats[3]);
+			tf3SetMax(m_floats[3], other.m_floats[3]);
 		}
   /**@brief Set each element to the min of the current values and the values of another QuadWord
    * @param other The other QuadWord to compare with 
    */
 		TF3SIMD_FORCE_INLINE void	setMin(const QuadWord& other)
 		{
-			// tf3SetMin(m_floats[0], other.m_floats[0]);
+			tf3SetMin(m_floats[0], other.m_floats[0]);
-			// tf3SetMin(m_floats[1], other.m_floats[1]);
+			tf3SetMin(m_floats[1], other.m_floats[1]);
-			// tf3SetMin(m_floats[2], other.m_floats[2]);
+			tf3SetMin(m_floats[2], other.m_floats[2]);
-			// tf3SetMin(m_floats[3], other.m_floats[3]);
+			tf3SetMin(m_floats[3], other.m_floats[3]);
 		}
 
 

src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h

@@ -27,495 +27,494 @@
 #include <robot/time.h>
 #include <move_base_core/common.h>
 
-
 namespace robot
 {
-namespace move_base_core
+	namespace move_base_core
-{
-	// Navigation states, including planning and controller status
-	enum class State
 	{
-		PENDING,
+		// Navigation states, including planning and controller status
-		ACTIVE,
+		enum class State
-		PREEMPTED,
-		SUCCEEDED,
-		ABORTED,
-		REJECTED,
-		PREEMPTING,
-		RECALLING,
-		RECALLED,
-		LOST,
-		PLANNING,
-		CONTROLLING,
-		CLEARING,
-		PAUSED
-	};
-
-	/**
-	 * @brief Feedback structure to describe current navigation status
-	 */
-	struct NavFeedback
-	{
-		State navigation_state;						// Current navigation state
-		std::string feed_back_str;					// Description or debug message
-		robot_geometry_msgs::Pose2D current_pose; 	// Robot’s current pose in 2D
-		bool goal_checked;							// Whether the current goal is verified
-		bool is_ready;								// Robot is ready for commands
-	};
-
-	/**
-	 * @brief Planner data output structure.
-	 */
-	struct PlannerDataOutput 
-	{
-		robot_nav_2d_msgs::Path2D plan;
-		robot_nav_msgs::OccupancyGrid costmap;
-		robot_map_msgs::OccupancyGridUpdate costmap_update;
-		bool is_costmap_updated;
-		robot_geometry_msgs::PolygonStamped footprint;
-	};
-
-	/**
-	 * @brief Convert a State enum to its string representation.
-	 *        Useful for debugging/logging or displaying in UI.
-	 *
-	 * @param state Enum value of move_base_core::State
-	 * @return std::string The corresponding string, e.g. "PENDING"
-	 */
-	inline std::string toString(move_base_core::State state)
-	{
-		using move_base_core::State;
-		switch (state)
 		{
-		case State::PENDING:
+			PENDING,
-			return "PENDING"; // Chờ xử lý
+			ACTIVE,
-		case State::ACTIVE:
+			PREEMPTED,
-			return "ACTIVE"; // Đang hoạt động
+			SUCCEEDED,
-		case State::PREEMPTED:
+			ABORTED,
-			return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
+			REJECTED,
-		case State::SUCCEEDED:
+			PREEMPTING,
-			return "SUCCEEDED"; // Thành công
+			RECALLING,
-		case State::ABORTED:
+			RECALLED,
-			return "ABORTED"; // Bị lỗi
+			LOST,
-		case State::REJECTED:
+			PLANNING,
-			return "REJECTED"; // Từ chối bởi planner hoặc controller
+			CONTROLLING,
-		case State::PREEMPTING:
+			CLEARING,
-			return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
+			PAUSED
-		case State::RECALLING:
+		};
-			return "RECALLING"; // Đang huỷ bỏ nội bộ
+
-		case State::RECALLED:
+		/**
-			return "RECALLED"; // Đã được thu hồi
+		 * @brief Feedback structure to describe current navigation status
-		case State::LOST:
+		 */
-			return "LOST"; // Mất trạng thái
+		struct NavFeedback
-		case State::PLANNING:
+		{
-			return "PLANNING"; // Đang lập kế hoạch đường đi
+			State navigation_state;										// Current navigation state
-		case State::CONTROLLING:
+			std::string feed_back_str;								// Description or debug message
-			return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
+			robot_geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D
-		case State::CLEARING:
+			bool goal_checked;												// Whether the current goal is verified
-			return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
+			bool is_ready;														// Robot is ready for commands
-		case State::PAUSED:
+		};
-			return "PAUSED"; // Tạm dừng
+
-		default:
+		/**
-			return "UNKNOWN_STATE"; // Không xác định
+		 * @brief Planner data output structure.
+		 */
+		struct PlannerDataOutput
+		{
+			robot_nav_2d_msgs::Path2D plan;
+			robot_nav_msgs::OccupancyGrid costmap;
+			robot_map_msgs::OccupancyGridUpdate costmap_update;
+			bool is_costmap_updated;
+			robot_geometry_msgs::PolygonStamped footprint;
+		};
+
+		/**
+		 * @brief Convert a State enum to its string representation.
+		 *        Useful for debugging/logging or displaying in UI.
+		 *
+		 * @param state Enum value of move_base_core::State
+		 * @return std::string The corresponding string, e.g. "PENDING"
+		 */
+		inline std::string toString(move_base_core::State state)
+		{
+			using move_base_core::State;
+			switch (state)
+			{
+			case State::PENDING:
+				return "PENDING"; // Chờ xử lý
+			case State::ACTIVE:
+				return "ACTIVE"; // Đang hoạt động
+			case State::PREEMPTED:
+				return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
+			case State::SUCCEEDED:
+				return "SUCCEEDED"; // Thành công
+			case State::ABORTED:
+				return "ABORTED"; // Bị lỗi
+			case State::REJECTED:
+				return "REJECTED"; // Từ chối bởi planner hoặc controller
+			case State::PREEMPTING:
+				return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
+			case State::RECALLING:
+				return "RECALLING"; // Đang huỷ bỏ nội bộ
+			case State::RECALLED:
+				return "RECALLED"; // Đã được thu hồi
+			case State::LOST:
+				return "LOST"; // Mất trạng thái
+			case State::PLANNING:
+				return "PLANNING"; // Đang lập kế hoạch đường đi
+			case State::CONTROLLING:
+				return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
+			case State::CLEARING:
+				return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
+			case State::PAUSED:
+				return "PAUSED"; // Tạm dừng
+			default:
+				return "UNKNOWN_STATE"; // Không xác định
+			}
 		}
-	}
-
-	/**
-	 * @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction.
-	 *
-	 * This function calculates a new position by moving forward (or backward if negative)
-	 * a certain `offset_distance` from the current position, following the given heading angle (theta).
-	 *
-	 * @param pose             The original 2D pose (x, y, theta) in the local plane.
-	 * @param frame_id         The coordinate frame in which the output PoseStamped will be expressed.
-	 * @param offset_distance  The distance to offset along the heading direction, in meters.
-	 *                         Positive moves forward, negative moves backward.
-	 * @return                 A new PoseStamped offset from the input pose, in the given frame.
-	 */
-	inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
-	{
-		robot_geometry_msgs::PoseStamped goal;
-		goal.header.frame_id = frame_id;
-		goal.header.stamp = robot::Time::now();
-		goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
-		goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
-
-		tf3::Quaternion q;
-		q.setRPY(0, 0, pose.theta);
-		goal.pose.orientation = tf3::toMsg(q);
-		return goal;
-	}
-
-	/**
-	 * @brief Overloaded version: creates an offset target pose from a given PoseStamped.
-	 *
-	 * This function extracts the 2D position and orientation (yaw) from the given PoseStamped,
-	 * offsets it forward (or backward) along the current heading direction,
-	 * and returns a new PoseStamped in the same frame.
-	 *
-	 * @param pose             The original pose with full position and orientation.
-	 * @param offset_distance  Distance to offset along the current heading direction (in meters).
-	 * @return                 A new PoseStamped offset from the original pose.
-	 */
-	inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance)
-	{
-		robot_geometry_msgs::Pose2D pose2d;
-		pose2d.x = pose.pose.position.x;
-		pose2d.y = pose.pose.position.y;
-		// pose2d.theta = tf2::getYaw(pose.pose.orientation);
-		return offset_goal(pose2d, pose.header.frame_id, offset_distance);
-	}
-
-	/**
-	 * @class BaseNavigation
-	 * @brief Abstract interface for robot navigation modules.
-	 *
-	 * Provides core methods for setting goals, moving, rotating, and handling motion control.
-	 * All navigation logic must implement this interface.
-	 */
-	class BaseNavigation
-	{
-	public:
-		using Ptr = std::shared_ptr<BaseNavigation>;
-
-		virtual ~BaseNavigation() {}
 
 		/**
-		 * @brief Initialize the navigation system.
+		 * @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction.
-		 * @param tf Shared pointer to the TF listener or buffer.
-		 */
-		virtual void initialize(TFListenerPtr tf) = 0;
-
-		/**
-		 * @brief Set the robot's footprint (outline shape) in the global frame.
-		 *        This can be used for planning or collision checking.
 		 *
-		 * @param fprt A vector of points representing the robot's footprint polygon.
+		 * This function calculates a new position by moving forward (or backward if negative)
-		 *             The points should be ordered counter-clockwise.
+		 * a certain `offset_distance` from the current position, following the given heading angle (theta).
-		 * Example:
 		 *
-			^ Y
+		 * @param pose             The original 2D pose (x, y, theta) in the local plane.
-			|
+		 * @param frame_id         The coordinate frame in which the output PoseStamped will be expressed.
-			|      P3(-0.3, 0.2)        P2(0.3, 0.2)
+		 * @param offset_distance  The distance to offset along the heading direction, in meters.
-			|         ●---------------●
+		 *                         Positive moves forward, negative moves backward.
-			|         |               |
+		 * @return                 A new PoseStamped offset from the input pose, in the given frame.
-			|         |     Robot     |   (view Top )
-			|         |               |
-			|         ●---------------●
-			|      P4(-0.3, -0.2)       P1(0.3, -0.2)
-			+-------------------------------> X
-
-			std::vector<robot_geometry_msgs::Point> footprint;
-			1. footprint.push_back(make_point(0.3, -0.2));
-			2. footprint.push_back(make_point(0.3, 0.2));
-			3. footprint.push_back(make_point(-0.3, 0.2));
-			4. footprint.push_back(make_point(-0.3, -0.2));
 		 */
-		virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) = 0;
+		inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
+		{
+			robot_geometry_msgs::PoseStamped goal;
+			goal.header.frame_id = frame_id;
+			goal.header.stamp = robot::Time::now();
+			goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
+			goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
+
+			tf3::Quaternion q;
+			q.setRPY(0, 0, pose.theta);
+			goal.pose.orientation = tf3::toMsg(q);
+			return goal;
+		}
 
 		/**
-		 * @brief Get the robot's footprint (outline shape) in the global frame.
+		 * @brief Overloaded version: creates an offset target pose from a given PoseStamped.
-		 * @return A vector of points representing the robot's footprint polygon.
+		 *
+		 * This function extracts the 2D position and orientation (yaw) from the given PoseStamped,
+		 * offsets it forward (or backward) along the current heading direction,
+		 * and returns a new PoseStamped in the same frame.
+		 *
+		 * @param pose             The original pose with full position and orientation.
+		 * @param offset_distance  Distance to offset along the current heading direction (in meters).
+		 * @return                 A new PoseStamped offset from the original pose.
 		 */
-		virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() = 0;
+		inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance)
+		{
+			robot_geometry_msgs::Pose2D pose2d;
+			pose2d.x = pose.pose.position.x;
+			pose2d.y = pose.pose.position.y;
+			// pose2d.theta = tf2::getYaw(pose.pose.orientation);
+			return offset_goal(pose2d, pose.header.frame_id, offset_distance);
+		}
 
 		/**
-		 * @brief Add a static map to the navigation system.
+		 * @class BaseNavigation
-		 * @param map_name The name of the map.
+		 * @brief Abstract interface for robot navigation modules.
-		 * @param map The map to add.
+		 *
+		 * Provides core methods for setting goals, moving, rotating, and handling motion control.
+		 * All navigation logic must implement this interface.
 		 */
-		 virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) = 0;
+		class BaseNavigation
+		{
+		public:
+			using Ptr = std::shared_ptr<BaseNavigation>;
 
-		 /**
+			virtual ~BaseNavigation() {}
-			* @brief Add a laser scan to the navigation system.
-			* @param laser_scan_name The name of the laser scan.
-			* @param laser_scan The laser scan to add.
-			*/
-		 virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) = 0;
 
-		 /**
+			/**
-			* @brief Add a point cloud to the navigation system.
+			 * @brief Initialize the navigation system.
-			* @param point_cloud_name The name of the point cloud.
+			 * @param tf Shared pointer to the TF listener or buffer.
-			* @param point_cloud The point cloud to add.
+			 */
-			*/
+			virtual void initialize(TFListenerPtr tf) = 0;
-		 virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) = 0;
 
-		 /**
+			/**
-			* @brief Add a point cloud2 to the navigation system.
+			 * @brief Set the robot's footprint (outline shape) in the global frame.
-			* @param point_cloud2_name The name of the point cloud2.
+			 *        This can be used for planning or collision checking.
-			* @param point_cloud2 The point cloud2 to add.
+			 *
-			*/
+			 * @param fprt A vector of points representing the robot's footprint polygon.
-		 virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0;
+			 *             The points should be ordered counter-clockwise.
+			 * Example:
+			 *
+				^ Y
+				|
+				|      P3(-0.3, 0.2)        P2(0.3, 0.2)
+				|         ●---------------●
+				|         |               |
+				|         |     Robot     |   (view Top )
+				|         |               |
+				|         ●---------------●
+				|      P4(-0.3, -0.2)       P1(0.3, -0.2)
+				+-------------------------------> X
 
-		 /**
+				std::vector<robot_geometry_msgs::Point> footprint;
-			* @brief Get a static map from the navigation system.
+				1. footprint.push_back(make_point(0.3, -0.2));
-			* @param map_name The name of the map.
+				2. footprint.push_back(make_point(0.3, 0.2));
-			* @return The map.
+				3. footprint.push_back(make_point(-0.3, 0.2));
-			*/
+				4. footprint.push_back(make_point(-0.3, -0.2));
-		 virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) = 0;
+			 */
+			virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) = 0;
 
-		 /**
+			/**
-			* @brief Get a laser scan from the navigation system.
+			 * @brief Get the robot's footprint (outline shape) in the global frame.
-			* @param laser_scan_name The name of the laser scan.
+			 * @return A vector of points representing the robot's footprint polygon.
-			* @return The laser scan.
+			 */
-			*/
+			virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() = 0;
-		 virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) = 0;
 
-		 /**
+			/**
-			* @brief Get a point cloud from the navigation system.
+			 * @brief Add a static map to the navigation system.
-			* @param point_cloud_name The name of the point cloud.
+			 * @param map_name The name of the map.
-			* @return The point cloud.
+			 * @param map The map to add.
-			*/
+			 */
-		 virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) = 0;
+			virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) = 0;
 
-		 /**
+			/**
-			* @brief Get a point cloud2 from the navigation system.
+			 * @brief Add a laser scan to the navigation system.
-			* @param point_cloud2_name The name of the point cloud2.
+			 * @param laser_scan_name The name of the laser scan.
-			* @return The point cloud2.
+			 * @param laser_scan The laser scan to add.
-			*/
+			 */
-		 virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) = 0;
+			virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) = 0;
 
-		 /**
+			/**
-			* @brief Get all static maps from the navigation system.
+			 * @brief Add a point cloud to the navigation system.
-			* @return The static maps.
+			 * @param point_cloud_name The name of the point cloud.
-			*/
+			 * @param point_cloud The point cloud to add.
-		 virtual std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() = 0;
+			 */
+			virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) = 0;
 
-		 /**
+			/**
-			* @brief Get all laser scans from the navigation system.
+			 * @brief Add a point cloud2 to the navigation system.
-			* @return The laser scans.
+			 * @param point_cloud2_name The name of the point cloud2.
-			*/
+			 * @param point_cloud2 The point cloud2 to add.
-		 virtual std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() = 0;
+			 */
+			virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0;
 
-		 /**
+			/**
-			* @brief Get all point clouds from the navigation system.
+			 * @brief Get a static map from the navigation system.
-			* @return The point clouds.
+			 * @param map_name The name of the map.
-			*/
+			 * @return The map.
-		 virtual std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() = 0;
+			 */
+			virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) = 0;
 
-		 /**
+			/**
-			* @brief Get all point cloud2s from the navigation system.
+			 * @brief Get a laser scan from the navigation system.
-			* @return The point cloud2s.
+			 * @param laser_scan_name The name of the laser scan.
-			*/
+			 * @return The laser scan.
-		 virtual std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() = 0;
+			 */
+			virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) = 0;
 
-		 /**
+			/**
-			* @brief Remove a static map from the navigation system.
+			 * @brief Get a point cloud from the navigation system.
-			* @param map_name The name of the map.
+			 * @param point_cloud_name The name of the point cloud.
-			* @return True if the map was removed successfully.
+			 * @return The point cloud.
-			*/
+			 */
-		 virtual bool removeStaticMap(const std::string &map_name) = 0;
+			virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) = 0;
 
-		 /**
+			/**
-			* @brief Remove a laser scan from the navigation system.
+			 * @brief Get a point cloud2 from the navigation system.
-			* @param laser_scan_name The name of the laser scan.
+			 * @param point_cloud2_name The name of the point cloud2.
-			* @return True if the laser scan was removed successfully.
+			 * @return The point cloud2.
-			*/
+			 */
-		 virtual bool removeLaserScan(const std::string &laser_scan_name) = 0;
+			virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) = 0;
 
-		 /**
+			/**
-			* @brief Remove a point cloud from the navigation system.
+			 * @brief Get all static maps from the navigation system.
-			* @param point_cloud_name The name of the point cloud.
+			 * @return The static maps.
-			* @return True if the point cloud was removed successfully.
+			 */
-			*/
+			virtual std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() = 0;
-		 virtual bool removePointCloud(const std::string &point_cloud_name) = 0;
 
-		 /**
+			/**
-			* @brief Remove a point cloud2 from the navigation system.
+			 * @brief Get all laser scans from the navigation system.
-			* @param point_cloud2_name The name of the point cloud2.
+			 * @return The laser scans.
-			* @return True if the point cloud2 was removed successfully.
+			 */
-			*/
+			virtual std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() = 0;
-		 virtual bool removePointCloud2(const std::string &point_cloud2_name) = 0;
 
-		 /**
+			/**
-			* @brief Remove all static maps from the navigation system.
+			 * @brief Get all point clouds from the navigation system.
-			* @return True if the static maps were removed successfully.
+			 * @return The point clouds.
-			*/
+			 */
-		 virtual bool removeAllStaticMaps() = 0;
+			virtual std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() = 0;
 
-		 /**
+			/**
-			* @brief Remove all laser scans from the navigation system.
+			 * @brief Get all point cloud2s from the navigation system.
-			* @return True if the laser scans were removed successfully.
+			 * @return The point cloud2s.
-			*/
+			 */
-		 virtual bool removeAllLaserScans() = 0;
+			virtual std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() = 0;
 
-		 /**
+			/**
-			* @brief Remove all point clouds from the navigation system.
+			 * @brief Remove a static map from the navigation system.
-			* @return True if the point clouds were removed successfully.
+			 * @param map_name The name of the map.
-			*/
+			 * @return True if the map was removed successfully.
-		 virtual bool removeAllPointClouds() = 0;
+			 */
+			virtual bool removeStaticMap(const std::string &map_name) = 0;
 
-		 /**
+			/**
-			* @brief Remove all point cloud2s from the navigation system.
+			 * @brief Remove a laser scan from the navigation system.
-			* @return True if the point cloud2s were removed successfully.
+			 * @param laser_scan_name The name of the laser scan.
-			*/
+			 * @return True if the laser scan was removed successfully.
-		 virtual bool removeAllPointCloud2s() = 0;
+			 */
+			virtual bool removeLaserScan(const std::string &laser_scan_name) = 0;
 
-		 /**
+			/**
-			* @brief Remove all data from the navigation system.
+			 * @brief Remove a point cloud from the navigation system.
-			* @return True if the data was removed successfully.
+			 * @param point_cloud_name The name of the point cloud.
-			*/
+			 * @return True if the point cloud was removed successfully.
-		 virtual bool removeAllData() = 0;
+			 */
+			virtual bool removePointCloud(const std::string &point_cloud_name) = 0;
 
-		/**
+			/**
-		 * @brief Add an odometry to the navigation system.
+			 * @brief Remove a point cloud2 from the navigation system.
-		 * @param odometry_name The name of the odometry.
+			 * @param point_cloud2_name The name of the point cloud2.
-		 * @param odometry The odometry to add.
+			 * @return True if the point cloud2 was removed successfully.
-		 */
+			 */
-		virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) = 0;
+			virtual bool removePointCloud2(const std::string &point_cloud2_name) = 0;
 
-		/**
+			/**
-		 * @brief Send a goal for the robot to navigate to.
+			 * @brief Remove all static maps from the navigation system.
-		 * @param goal              Target pose in the global frame.
+			 * @return True if the static maps were removed successfully.
-		 * @param xy_goal_tolerance Acceptable error in X/Y (meters).
+			 */
-		 * @param yaw_goal_tolerance Acceptable angular error (radians).
+			virtual bool removeAllStaticMaps() = 0;
-		 * @return True if goal was accepted and sent successfully.
-		 */
-		virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
-												double xy_goal_tolerance = 0.0,
-												double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Send a goal for the robot to navigate to.
+			 * @brief Remove all laser scans from the navigation system.
-		 * @param msg               Order message.
+			 * @return True if the laser scans were removed successfully.
-		 * @param goal              Target pose in the global frame.
+			 */
-		 * @param xy_goal_tolerance Acceptable error in X/Y (meters).
+			virtual bool removeAllLaserScans() = 0;
-		 * @param yaw_goal_tolerance Acceptable angular error (radians).
-		 * @return True if goal was accepted and sent successfully.
-		 */
-		virtual bool moveTo(const robot_protocol_msgs::Order &msg,
-												const robot_geometry_msgs::PoseStamped &goal,
-												double xy_goal_tolerance = 0.0,
-												double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Send a docking goal to a predefined marker (e.g. charger).
+			 * @brief Remove all point clouds from the navigation system.
-		 * @param maker             Marker name or ID.
+			 * @return True if the point clouds were removed successfully.
-		 * @param goal              Target pose for docking.
+			 */
-		 * @param xy_goal_tolerance Acceptable XY error (meters).
+			virtual bool removeAllPointClouds() = 0;
-		 * @param yaw_goal_tolerance Acceptable heading error (radians).
-		 * @return True if docking command succeeded.
-		 */
-		virtual bool dockTo(const std::string &maker,
-												const robot_geometry_msgs::PoseStamped &goal,
-												double xy_goal_tolerance = 0.0,
-												double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Send a docking goal to a predefined marker (e.g. charger).
+			 * @brief Remove all point cloud2s from the navigation system.
-		 * @param msg               Order message.
+			 * @return True if the point cloud2s were removed successfully.
-		 * @param goal              Target pose for docking.
+			 */
-		 * @param xy_goal_tolerance Acceptable XY error (meters).
+			virtual bool removeAllPointCloud2s() = 0;
-		 * @param yaw_goal_tolerance Acceptable heading error (radians).
-		 * @return True if docking command succeeded.
-		 */
-		virtual bool dockTo(const robot_protocol_msgs::Order &msg,
-												const robot_geometry_msgs::PoseStamped &goal,
-												double xy_goal_tolerance = 0.0,
-												double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Move straight toward the target position (X-axis).
+			 * @brief Remove all data from the navigation system.
-		 * @param goal              Target pose.
+			 * @return True if the data was removed successfully.
-		 * @param xy_goal_tolerance Acceptable positional error (meters).
+			 */
-		 * @return True if command issued successfully.
+			virtual bool removeAllData() = 0;
-		 */
-		virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
-																double xy_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Rotate in place to align with target orientation.
+			 * @brief Add an odometry to the navigation system.
-		 * @param goal              Pose containing desired heading (only Z-axis used).
+			 * @param odometry_name The name of the odometry.
-		 * @param yaw_goal_tolerance Acceptable angular error (radians).
+			 * @param odometry The odometry to add.
-		 * @return True if rotation command was sent successfully.
+			 */
-		 */
+			virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) = 0;
-		virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
+
+			/**
+			 * @brief Send a goal for the robot to navigate to.
+			 * @param goal              Target pose in the global frame.
+			 * @param xy_goal_tolerance Acceptable error in X/Y (meters).
+			 * @param yaw_goal_tolerance Acceptable angular error (radians).
+			 * @return True if goal was accepted and sent successfully.
+			 */
+			virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
+													double xy_goal_tolerance = 0.0,
 													double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Pause the robot's movement (e.g. during obstacle avoidance).
+			 * @brief Send a goal for the robot to navigate to.
-		 */
+			 * @param msg               Order message.
-		virtual void pause() = 0;
+			 * @param goal              Target pose in the global frame.
+			 * @param xy_goal_tolerance Acceptable error in X/Y (meters).
+			 * @param yaw_goal_tolerance Acceptable angular error (radians).
+			 * @return True if goal was accepted and sent successfully.
+			 */
+			virtual bool moveTo(const robot_protocol_msgs::Order &msg,
+													const robot_geometry_msgs::PoseStamped &goal,
+													double xy_goal_tolerance = 0.0,
+													double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Resume motion after a pause.
+			 * @brief Send a docking goal to a predefined marker (e.g. charger).
-		 */
+			 * @param maker             Marker name or ID.
-		virtual void resume() = 0;
+			 * @param goal              Target pose for docking.
+			 * @param xy_goal_tolerance Acceptable XY error (meters).
+			 * @param yaw_goal_tolerance Acceptable heading error (radians).
+			 * @return True if docking command succeeded.
+			 */
+			virtual bool dockTo(const std::string &maker,
+													const robot_geometry_msgs::PoseStamped &goal,
+													double xy_goal_tolerance = 0.0,
+													double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Cancel the current goal and stop the robot.
+			 * @brief Send a docking goal to a predefined marker (e.g. charger).
-		 */
+			 * @param msg               Order message.
-		virtual void cancel() = 0;
+			 * @param goal              Target pose for docking.
+			 * @param xy_goal_tolerance Acceptable XY error (meters).
+			 * @param yaw_goal_tolerance Acceptable heading error (radians).
+			 * @return True if docking command succeeded.
+			 */
+			virtual bool dockTo(const robot_protocol_msgs::Order &msg,
+													const robot_geometry_msgs::PoseStamped &goal,
+													double xy_goal_tolerance = 0.0,
+													double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Send limited linear velocity command.
+			 * @brief Move straight toward the target position (X-axis).
-		 * @param linear Linear velocity vector (x, y, z).
+			 * @param goal              Target pose.
-		 * @return True if the command was accepted.
+			 * @param xy_goal_tolerance Acceptable positional error (meters).
-		 */
+			 * @return True if command issued successfully.
-		virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0;
+			 */
+			virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
+																	double xy_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Send limited angular velocity command.
+			 * @brief Rotate in place to align with target orientation.
-		 * @param angular Angular velocity vector (x, y, z).
+			 * @param goal              Pose containing desired heading (only Z-axis used).
-		 * @return True if the command was accepted.
+			 * @param yaw_goal_tolerance Acceptable angular error (radians).
-		 */
+			 * @return True if rotation command was sent successfully.
-		virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) = 0;
+			 */
+			virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
+														double yaw_goal_tolerance = 0.0) = 0;
 
-		/**
+			/**
-		 * @brief Get the robot’s pose as a PoseStamped.
+			 * @brief Pause the robot's movement (e.g. during obstacle avoidance).
-		 * @param pose Output parameter with the robot’s current pose.
+			 */
-		 * @return True if pose was successfully retrieved.
+			virtual void pause() = 0;
-		 */
-		virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) = 0;
 
-		/**
+			/**
-		 * @brief Get the robot’s pose as a 2D pose.
+			 * @brief Resume motion after a pause.
-		 * @param pose Output parameter with the robot’s current 2D pose.
+			 */
-		 * @return True if pose was successfully retrieved.
+			virtual void resume() = 0;
-		 */
-		virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) = 0;
 
-		/**
+			/**
-		 * @brief Get the robot’s twist.
+			 * @brief Cancel the current goal and stop the robot.
-		 * @return The robot’s current twist.
+			 */
-		 */
+			virtual void cancel() = 0;
-		virtual robot_nav_2d_msgs::Twist2DStamped getTwist() = 0;
 
-		/**
+			/**
-		 * @brief Get the navigation feedback.
+			 * @brief Send limited linear velocity command.
-		 * @return Pointer to the navigation feedback.
+			 * @param linear Linear velocity vector (x, y, z).
-		 */
+			 * @return True if the command was accepted.
-		virtual NavFeedback *getFeedback() = 0;
+			 */
+			virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0;
 
-		/**
+			/**
-		 * @brief Get the global data.
+			 * @brief Send limited angular velocity command.
-		 * @return The global data.
+			 * @param angular Angular velocity vector (x, y, z).
-		 */
+			 * @return True if the command was accepted.
-		virtual PlannerDataOutput getGlobalData() = 0;
+			 */
+			virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) = 0;
 
-		/**
+			/**
-		 * @brief Get the local data.
+			 * @brief Get the robot’s pose as a PoseStamped.
-		 * @return The local data.
+			 * @param pose Output parameter with the robot’s current pose.
-		 */
+			 * @return True if pose was successfully retrieved.
-		virtual PlannerDataOutput getLocalData() = 0;
+			 */
+			virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) = 0;
 
-	protected:
+			/**
-		// Shared feedback data for navigation status tracking
+			 * @brief Get the robot’s pose as a 2D pose.
-		std::shared_ptr<NavFeedback> nav_feedback_;
+			 * @param pose Output parameter with the robot’s current 2D pose.
-		robot_nav_2d_msgs::Twist2DStamped twist_;
+			 * @return True if pose was successfully retrieved.
-		robot_nav_msgs::Odometry odometry_;
+			 */
-		PlannerDataOutput global_data_;
+			virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) = 0;
-		PlannerDataOutput local_data_;
 
-		std::map<std::string, robot_nav_msgs::OccupancyGrid> static_maps_;
+			/**
-		std::map<std::string, robot_sensor_msgs::LaserScan> laser_scans_;
+			 * @brief Get the robot’s twist.
-		std::map<std::string, robot_sensor_msgs::PointCloud> point_clouds_;
+			 * @return The robot’s current twist.
-		std::map<std::string, robot_sensor_msgs::PointCloud2> point_cloud2s_;
+			 */
+			virtual robot_nav_2d_msgs::Twist2DStamped getTwist() = 0;
 
-		BaseNavigation() = default;
+			/**
-	};
+			 * @brief Get the navigation feedback.
+			 * @return Pointer to the navigation feedback.
+			 */
+			virtual NavFeedback *getFeedback() = 0;
 
-} // namespace move_base_core
+			/**
+			 * @brief Get the global data.
+			 * @return The global data.
+			 */
+			virtual PlannerDataOutput getGlobalData() = 0;
+
+			/**
+			 * @brief Get the local data.
+			 * @return The local data.
+			 */
+			virtual PlannerDataOutput getLocalData() = 0;
+
+		protected:
+			// Shared feedback data for navigation status tracking
+			std::shared_ptr<NavFeedback> nav_feedback_;
+			robot_nav_2d_msgs::Twist2DStamped twist_;
+			robot_nav_msgs::Odometry odometry_;
+			PlannerDataOutput global_data_;
+			PlannerDataOutput local_data_;
+
+			std::map<std::string, robot_nav_msgs::OccupancyGrid> static_maps_;
+			std::map<std::string, robot_sensor_msgs::LaserScan> laser_scans_;
+			std::map<std::string, robot_sensor_msgs::PointCloud> point_clouds_;
+			std::map<std::string, robot_sensor_msgs::PointCloud2> point_cloud2s_;
+
+			BaseNavigation() = default;
+		};
+
+	} // namespace move_base_core
 };
 
 #endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h

@@ -133,6 +133,13 @@ namespace robot_nav_core
      */
     virtual bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) = 0;
 
+    /**
+     * @brief Gets the global plan for this local planner.
+     *
+     * @param path The global plan
+     */ 
+    virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
+
     /**
      * @brief  Constructs the local planner
      * @param name The name to give this instance of the local planner

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h

@@ -92,6 +92,13 @@ namespace robot_nav_core2
      */
     virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0;
 
+    /**
+     * @brief Gets the global plan for this local planner.
+     *
+     * @param path The global plan
+     */ 
+    virtual void getPlan(robot_nav_2d_msgs::Path2D &path) = 0;
+
     /**
      * @brief Compute the best command given the current pose, velocity and goal
      *

src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h

@@ -159,6 +159,14 @@ namespace robot_nav_core_adapter
      */
     bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) override;
 
+
+    /**
+     * @brief Gets the global plan for this local planner.
+     *
+     * @param path The global plan
+     */ 
+    virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
+
     /**
      * @brief Create a new LocalPlannerAdapter
      * @return A shared pointer to the new LocalPlannerAdapter

src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp

@@ -366,6 +366,17 @@ namespace robot_nav_core_adapter
     }
   }
 
+  void LocalPlannerAdapter::getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
+  {
+    if (!planner_)
+    {
+      return;
+    }
+    robot_nav_2d_msgs::Path2D path2d;
+    planner_->getPlan(path2d);
+    path = robot_nav_2d_utils::pathToPath(path2d).poses;
+  }
+
   bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
   {
     if (last_goal_.header.frame_id != new_goal.header.frame_id ||

src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt

@@ -30,7 +30,6 @@ if (NOT BUILDING_WITH_CATKIN)
 
   set(PACKAGES_DIR  
     robot_std_msgs
-    utils
     robot_time
   )
 
@@ -41,7 +40,6 @@ else()
 # ========================================================
   find_package(catkin REQUIRED COMPONENTS         
     robot_std_msgs
-    utils
     robot_time
   )
 
@@ -50,7 +48,7 @@ else()
   catkin_package(
     INCLUDE_DIRS include
     LIBRARIES ${PROJECT_NAME} 
-    CATKIN_DEPENDS robot_std_msgs utils robot_time
+    CATKIN_DEPENDS robot_std_msgs robot_time
     DEPENDS Boost
   )
 

src/Navigations/Libraries/robot_actionlib_msgs/package.xml

@@ -22,9 +22,6 @@
   <build_depend>robot_std_msgs</build_depend>
   <run_depend>robot_std_msgs</run_depend>
 
-  <build_depend>utils</build_depend>
-  <run_depend>utils</run_depend>
-
   <build_depend>robot_time</build_depend>
   <run_depend>robot_time</run_depend>
 </package>

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

@@ -607,7 +607,6 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L
 
 void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry)
 {
-  robot::log_info("[%s:%d] addOdometry called for: %s", __FILE__, __LINE__, odometry_name.c_str());
   odometry_ = odometry;
 }
 
@@ -2206,6 +2205,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       {
         if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
         {
+          robot_nav_msgs::Path path;
+          tc_->getPlan(path.poses);
+          if (!path.poses.empty())
+          {
+            path.header.stamp = path.poses[0].header.stamp;
+            path.header.frame_id = path.poses[0].header.frame_id;
+          }
+          else
+          {
+            path.header.stamp = robot::Time::now();
+            path.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
+          }
+          local_data_.plan = robot_nav_2d_utils::pathToPath(path);
+
           last_valid_control_ = robot::Time::now();
           // make sure that we send the velocity command to the base
 
@@ -2283,6 +2296,9 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
             lock.unlock();
           }
         }
+        twist_.velocity = robot_nav_2d_utils::twist3Dto2D(cmd_vel);
+        twist_.header.stamp = robot::Time::now();
+        twist_.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
       }
       else
       {