update init

.gitmodules

@@ -26,3 +26,9 @@
 [submodule "src/Libraries/robot_cpp"]
 	path = src/Libraries/robot_cpp
 	url = http://git.pnkx/HiepLM/robot_cpp.git
+[submodule "src/Algorithms/Packages/global_planners/custom_planner"]
+	path = src/Algorithms/Packages/global_planners/custom_planner
+	url = https://git.pnkr.asia/DuongTD/custom_planner.git
+[submodule "src/Algorithms/Packages/global_planners/dock_planner"]
+	path = src/Algorithms/Packages/global_planners/dock_planner
+	url = https://git.pnkr.asia/DuongTD/dock_planner.git

.vscode/settings.json

@@ -1,71 +0,0 @@
-{
-    "files.associations": {
-        "stdexcept": "cpp",
-        "cctype": "cpp",
-        "clocale": "cpp",
-        "cmath": "cpp",
-        "csignal": "cpp",
-        "cstdarg": "cpp",
-        "cstddef": "cpp",
-        "cstdio": "cpp",
-        "cstdlib": "cpp",
-        "cstring": "cpp",
-        "ctime": "cpp",
-        "cwchar": "cpp",
-        "cwctype": "cpp",
-        "array": "cpp",
-        "atomic": "cpp",
-        "strstream": "cpp",
-        "bit": "cpp",
-        "bitset": "cpp",
-        "chrono": "cpp",
-        "compare": "cpp",
-        "complex": "cpp",
-        "concepts": "cpp",
-        "condition_variable": "cpp",
-        "cstdint": "cpp",
-        "deque": "cpp",
-        "list": "cpp",
-        "map": "cpp",
-        "set": "cpp",
-        "unordered_map": "cpp",
-        "vector": "cpp",
-        "exception": "cpp",
-        "algorithm": "cpp",
-        "functional": "cpp",
-        "iterator": "cpp",
-        "memory": "cpp",
-        "memory_resource": "cpp",
-        "numeric": "cpp",
-        "optional": "cpp",
-        "random": "cpp",
-        "ratio": "cpp",
-        "string": "cpp",
-        "string_view": "cpp",
-        "system_error": "cpp",
-        "tuple": "cpp",
-        "type_traits": "cpp",
-        "utility": "cpp",
-        "fstream": "cpp",
-        "initializer_list": "cpp",
-        "iomanip": "cpp",
-        "iosfwd": "cpp",
-        "iostream": "cpp",
-        "istream": "cpp",
-        "limits": "cpp",
-        "mutex": "cpp",
-        "new": "cpp",
-        "ostream": "cpp",
-        "ranges": "cpp",
-        "sstream": "cpp",
-        "stop_token": "cpp",
-        "streambuf": "cpp",
-        "thread": "cpp",
-        "cfenv": "cpp",
-        "cinttypes": "cpp",
-        "typeindex": "cpp",
-        "typeinfo": "cpp",
-        "variant": "cpp",
-        "codecvt": "cpp"
-    }
-}

CMAKE_BUILD_README.md

@@ -214,8 +214,7 @@ find_package(navigation_c_api REQUIRED)
 add_executable(my_app main.cpp)
 target_link_libraries(my_app
     PRIVATE
-        robot::node_handle
-        robot::console
+        robot_cpp
         move_base
         navigation_c_api
 )

CMakeLists.txt

@@ -114,6 +114,10 @@ if (NOT TARGET score_algorithm)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
 endif()
 
+if (NOT TARGET mkt_plugins)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins)
+endif()
+
 if (NOT TARGET mkt_algorithm)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
 endif()
@@ -122,6 +126,18 @@ if (NOT TARGET two_points_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)
 endif()
 
+if (NOT TARGET custom_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner)
+endif()
+
+if (NOT TARGET dock_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner)
+endif()
+
+if (NOT TARGET pnkx_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
+endif()
+
 
 # 2. Main packages (phụ thuộc vào cores)
 message(STATUS "[move_base] Shared library configured")

config/costmap_global_params.yaml

@@ -1,13 +1,14 @@
 global_costmap:
-    global_frame: map
-    update_frequency: 1.0
-    publish_frequency: 1.0
-    raytrace_range: 2.0
-    resolution: 0.05
-    z_resolution: 0.2
-    rolling_window: false
-    z_voxels: 10
-    inflation:
-      cost_scaling_factor:  10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
-      inflation_radius:     0.6  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
+  path_plugins: /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Libraries/costmap_2d/libplugins.so
+  global_frame: map
+  update_frequency: 1.0
+  publish_frequency: 1.0
+  raytrace_range: 2.0
+  resolution: 0.05
+  z_resolution: 0.2
+  rolling_window: false
+  z_voxels: 10
+  inflation:
+    cost_scaling_factor:  10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
+    inflation_radius:     0.6  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
 

config/costmap_global_params_plugins_no_virtual_walls.yaml

@@ -1,10 +1,10 @@
 global_costmap:
     frame_id: map
     plugins:
-        - {name: navigation_map, type: "costmap_2d::StaticLayer" }
-        - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" }
-        - {name: obstacles,  type: "costmap_2d::VoxelLayer" }
-        - {name: inflation,  type: "costmap_2d::InflationLayer" }
+        - {name: navigation_map, type: "StaticLayer" }
+        - {name: virtual_walls_map, type: "StaticLayer" }
+        - {name: obstacles,  type: "VoxelLayer" }
+        - {name: inflation,  type: "InflationLayer" }
     obstacles:
         enabled: false
         footprint_clearing_enabled: false

config/costmap_local_params.yaml

@@ -1,16 +1,17 @@
 local_costmap:  
-    global_frame: odom
-    update_frequency: 6.0
-    publish_frequency: 6.0
-    rolling_window: true
-    raytrace_range: 2.0
-    resolution: 0.05
-    z_resolution: 0.15
-    z_voxels: 8
-    inflation:
-      cost_scaling_factor: 10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
-      inflation_radius: 0.3  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
-    width: 8.0
-    height: 8.0
-    origin_x: 0.0
-    origin_y: 0.0
+  path_plugins: /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Libraries/costmap_2d/libplugins.so
+  global_frame: odom
+  update_frequency: 6.0
+  publish_frequency: 6.0
+  rolling_window: true
+  raytrace_range: 2.0
+  resolution: 0.05
+  z_resolution: 0.15
+  z_voxels: 8
+  inflation:
+    cost_scaling_factor: 10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
+    inflation_radius: 0.3  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
+  width: 8.0
+  height: 8.0
+  origin_x: 0.0
+  origin_y: 0.0

config/costmap_local_params_plugins_no_virtual_walls.yaml

@@ -1,8 +1,8 @@
 local_costmap:
-    # frame_id: odom
-    # plugins:
-    #     - {name: obstacles,  type: "costmap_2d::VoxelLayer" }
-    #     - {name: inflation,  type: "costmap_2d::InflationLayer" }
-    obstacles:
-        enabled: true
-        footprint_clearing_enabled: true
+  frame_id: odom
+  plugins:
+    - {name: obstacles,  type: "VoxelLayer" }
+    - {name: inflation,  type: "InflationLayer" }
+  obstacles:
+    enabled: true
+    footprint_clearing_enabled: true

config/pnkx_local_planner_params.yaml

@@ -14,33 +14,33 @@ publish_topic: true
 LocalPlannerAdapter:
   PNKXLocalPlanner:
     # Algorithm
-    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
-    algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
-    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
+    trajectory_generator_name: LimitedAccelGenerator 
+    algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
+    algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
     # Goal checking
-    goal_checker_name: mkt_plugins::GoalChecker 
+    goal_checker_name: GoalChecker 
 
   PNKXDockingLocalPlanner: 
     # Algorithm
-    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
-    algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
-    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
+    trajectory_generator_name: LimitedAccelGenerator 
+    algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
+    algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
     # Goal checking
-    goal_checker_name: mkt_plugins::GoalChecker 
+    goal_checker_name: GoalChecker 
 
   PNKXGoStraightLocalPlanner:
     # Algorithm
-    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
-    algorithm_nav_name: mkt_algorithm::diff::GoStraight
+    trajectory_generator_name: LimitedAccelGenerator 
+    algorithm_nav_name: MKTAlgorithmDiffGoStraight
     # Goal checking
-    goal_checker_name: mkt_plugins::GoalChecker 
+    goal_checker_name: GoalChecker 
 
   PNKXRotateLocalPlanner:
     # Algorithm
-    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
-    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
+    trajectory_generator_name: LimitedAccelGenerator 
     # Goal checking
-    goal_checker_name: mkt_plugins::SimpleGoalChecker
+    goal_checker_name: SimpleGoalChecker
     stateful: true
     
 LimitedAccelGenerator:

examples/CSharpExample.cs

@@ -159,6 +159,15 @@ namespace NavigationExample
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         public static extern void tf_listener_destroy(IntPtr handle);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf_listener_set_static_transform(
+            IntPtr tf_handle,
+            string parent_frame,
+            string child_frame,
+            double x, double y, double z,
+            double qx, double qy, double qz, double qw);
+
         // ============================================================================
         // Navigation Interface Methods
         // ============================================================================
@@ -267,6 +276,26 @@ namespace NavigationExample
                 return;
             }
 
+            // Inject a static TF so costmap can immediately canTransform(map <-> base_link).
+            // If you already publish TF from localization/odometry, you can remove this call.
+            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "map", "odom",
+                                                               0, 0, 0,
+                                                               0, 0, 0, 1))
+            {
+                LogError("Failed to inject static TF map -> odom");
+                NavigationAPI.tf_listener_destroy(tfHandle);
+                return;
+            }
+
+                        if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_link",
+                                                               0, 0, 0,
+                                                               0, 0, 0, 1))
+            {
+                LogError("Failed to inject static TF map -> base_link");
+                NavigationAPI.tf_listener_destroy(tfHandle);
+                return;
+            }
+
             // Create navigation instance
             IntPtr navHandle = NavigationAPI.navigation_create();
             if (navHandle == IntPtr.Zero)

examples/NavigationExample/Program.cs

@@ -159,6 +159,15 @@ namespace NavigationExample
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         public static extern void tf_listener_destroy(IntPtr handle);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf_listener_set_static_transform(
+            IntPtr tf_handle,
+            string parent_frame,
+            string child_frame,
+            double x, double y, double z,
+            double qx, double qy, double qz, double qw);
+
         // ============================================================================
         // Navigation Interface Methods
         // ============================================================================
@@ -267,6 +276,26 @@ namespace NavigationExample
                 return;
             }
 
+            // Inject a static TF so costmap can immediately canTransform(map <-> base_link).
+            // If you already publish TF from localization/odometry, you can remove this call.
+            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "map", "odom",
+                                                               0, 0, 0,
+                                                               0, 0, 0, 1))
+            {
+                LogError("Failed to inject static TF map -> odom");
+                NavigationAPI.tf_listener_destroy(tfHandle);
+                return;
+            }
+
+                        if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_link",
+                                                               0, 0, 0,
+                                                               0, 0, 0, 1))
+            {
+                LogError("Failed to inject static TF map -> base_link");
+                NavigationAPI.tf_listener_destroy(tfHandle);
+                return;
+            }
+
             // Create navigation instance
             IntPtr navHandle = NavigationAPI.navigation_create();
             if (navHandle == IntPtr.Zero)

src/APIs/c_api/include/nav_c_api.h

@@ -205,6 +205,30 @@ TFListenerHandle tf_listener_create(void);
  */
 void tf_listener_destroy(TFListenerHandle handle);
 
+/**
+ * @brief Inject a static transform into the TF buffer.
+ *
+ * This is a convenience for standalone usage where no external TF publisher exists yet.
+ * It will create/ensure the frames exist and become transformable.
+ *
+ * @param tf_handle TF listener handle
+ * @param parent_frame Parent frame id (e.g. "map")
+ * @param child_frame Child frame id (e.g. "base_link")
+ * @param x Translation x (meters)
+ * @param y Translation y (meters)
+ * @param z Translation z (meters)
+ * @param qx Rotation quaternion x
+ * @param qy Rotation quaternion y
+ * @param qz Rotation quaternion z
+ * @param qw Rotation quaternion w
+ * @return true on success, false on failure
+ */
+bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
+                                     const char* parent_frame,
+                                     const char* child_frame,
+                                     double x, double y, double z,
+                                     double qx, double qy, double qz, double qw);
+
 // ============================================================================
 // Navigation Interface Methods
 // ============================================================================

src/APIs/c_api/src/nav_c_api.cpp

@@ -178,19 +178,53 @@ extern "C" void tf_listener_destroy(TFListenerHandle handle) {
     }
 }
 
+extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
+                                                 const char* parent_frame,
+                                                 const char* child_frame,
+                                                 double x, double y, double z,
+                                                 double qx, double qy, double qz, double qw)
+{
+    if (!tf_handle || !parent_frame || !child_frame) {
+        return false;
+    }
+
+    try {
+        auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
+        if (!tf_ptr || !(*tf_ptr)) {
+            return false;
+        }
+
+        tf3::TransformStampedMsg st;
+        st.header.stamp = tf3::Time::now();
+        st.header.frame_id = parent_frame;
+        st.child_frame_id = child_frame;
+        st.transform.translation.x = x;
+        st.transform.translation.y = y;
+        st.transform.translation.z = z;
+        st.transform.rotation.x = qx;
+        st.transform.rotation.y = qy;
+        st.transform.rotation.z = qz;
+        st.transform.rotation.w = qw;
+
+        return (*tf_ptr)->setTransform(st, "nav_c_api(static)", true /*is_static*/);
+    } catch (...) {
+        return false;
+    }
+}
+
 // ============================================================================
 // Navigation Interface Methods
 // ============================================================================
 
 extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) 
 {
-    printf("[%s:%d] Begin: navigation_initialize\n", __FILE__, __LINE__);
+    printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__);
     if (!handle || !tf_handle) {
-        printf("[%s:%d] Error: Invalid parameters\n", __FILE__, __LINE__);
+        printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__);
         return false;
     }
     try {
-        printf("[%s:%d] Initialize navigation\n", __FILE__, __LINE__);
+        printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
         move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
         auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
         
@@ -200,19 +234,19 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
             boost::dll::load_mode::append_decorations);
             
         move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
-        printf("[%s:%d] Navigation created\n", __FILE__, __LINE__);
+        printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__);
         if (nav_ptr == nullptr) {
-            printf("[%s:%d] Error: Failed to create navigation\n", __FILE__, __LINE__);
+            printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__);
             return false;
         }
         nav_ptr->initialize(*tf_ptr);
     
         return true;    
     } catch (const std::exception &e) {
-        printf("[%s:%d] Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what());
+        printf("[%s:%d]\n Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what());
         return false;
     } catch (...) {
-        printf("[%s:%d] Error: Failed to initialize navigation\n", __FILE__, __LINE__);
+        printf("[%s:%d]\n Error: Failed to initialize navigation\n", __FILE__, __LINE__);
         return false;
     }
 }

src/Algorithms/Cores/score_algorithm/CMakeLists.txt

@@ -25,8 +25,7 @@ add_library(score_algorithm src/score_algorithm.cpp)
 target_link_libraries(score_algorithm 
   PUBLIC
     ${PACKAGES_DIR}
-    robot::node_handle
-    robot::console
+    robot_cpp
   PRIVATE
     Boost::filesystem
     Boost::system

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

@@ -1,7 +1,8 @@
 #ifndef _SCORE_GOAL_CHECKER_H_INCLUDED__
 #define _SCORE_GOAL_CHECKER_H_INCLUDED__
 
-#include <ros/ros.h>
+#include <robot/node_handle.h>
+#include <robot/console.h>
 #include <geometry_msgs/Pose2D.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <nav_2d_msgs/Twist2D.h>
@@ -32,7 +33,7 @@ namespace score_algorithm
      * @brief Initialize any parameters from the NodeHandle
      * @param nh NodeHandle for grabbing parameters
      */
-    virtual void initialize(const ros::NodeHandle& nh) = 0;
+    virtual void initialize(const robot::NodeHandle& nh) = 0;
 
     /**
      * @brief Reset the state of the goal checker (if any)

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

@@ -36,9 +36,9 @@ namespace score_algorithm
      * @brief Initialize parameters as needed
      * @param name Namespace for this planner
      * @param tf TFListener pointer
-     * @param costmap_ros Costmap pointer
+     * @param costmap_robot Costmap pointer
      */
-    virtual void initialize(robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+    virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
                             const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
   
     /**
@@ -154,10 +154,10 @@ namespace score_algorithm
     double angle_threshold_;
 
     bool enable_publish_;
-    // ros::Publisher reached_intermediate_goals_pub_;
-    // ros::Publisher current_goal_pub_;
-    // ros::Publisher closet_robot_goal_pub_;
-    // ros::Publisher transformed_plan_pub_, compute_plan_pub_;
+    // 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<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

@@ -31,7 +31,7 @@ namespace score_algorithm
   class TrajectoryGenerator
   {
   public:
-    using Ptr = boost::shared_ptr<score_algorithm::TrajectoryGenerator>;
+    using Ptr = std::shared_ptr<score_algorithm::TrajectoryGenerator>;
 
     virtual ~TrajectoryGenerator() {}
 

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

@@ -370,7 +370,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose
     bool goal_already_reached = false;
 
     // geometry_msgs::PoseArray poseArrayMsg;
-    // poseArrayMsg.header.stamp = ros::Time::now();
+    // poseArrayMsg.header.stamp = robot::Time::now();
     // poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
     // int c = 0;
     for (auto &reached_intermediate_goal : reached_intermediate_goals_)

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -52,8 +52,7 @@ add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS})
 target_link_libraries(mkt_algorithm_diff
     PUBLIC
         ${PACKAGES_DIR}
-        robot::node_handle
-        robot::console
+        robot_cpp
         Boost::system
         Eigen3::Eigen
 )

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h

@@ -27,7 +27,7 @@ public:
    * @brief Initialize parameters as needed
    * @param nh NodeHandle to read parameters from
    */
-  virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
 
   /**
    * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
@@ -70,9 +70,9 @@ protected:
                          
   bool findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
 
-  ros::NodeHandle nh_priv_, nh_;
+  robot::NodeHandle nh_priv_, nh_;
   std::string frame_id_path_;
-  ros::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;
+  robot::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;
 
   std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
   geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
@@ -98,8 +98,8 @@ protected:
   bool follow_step_path_;
   bool is_filter_;
   boost::shared_ptr<KalmanFilter> kf_;
-  ros::Publisher cmd_raw_pub_;
-  ros::Time last_actuator_update_;
+  robot::Publisher cmd_raw_pub_;
+  robot::Time last_actuator_update_;
 }; // class Bicycle 
 
 } // namespace mkt_algorithm

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h

@@ -25,7 +25,7 @@ public:
    * @brief Initialize parameters as needed
    * @param nh NodeHandle to read parameters from
    */
-  virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
 
   /**
    * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
@@ -55,13 +55,13 @@ public:
 
 
 protected:
-  ros::NodeHandle nh_;
-  ros::NodeHandle nh_kinematics_;
+  robot::NodeHandle nh_;
+  robot::NodeHandle nh_kinematics_;
 
   double steering_fix_wheel_distance_x_;
   double steering_fix_wheel_distance_y_;
   geometry_msgs::Pose2D goal_;
-  ros::Time time_last_cmd_;
+  robot::Time time_last_cmd_;
 
 }; // class GoStraight 
 

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h

@@ -14,7 +14,7 @@ public:
    * @brief Initialize any parameters from the NodeHandle
    * @param nh NodeHandle for grabbing parameters
    */
-  virtual void initialize(const ros::NodeHandle& nh) override;
+  virtual void initialize(const robot::NodeHandle& nh) override;
 
   /**
    * @brief Reset the state of the goal checker (if any)

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h

@@ -24,7 +24,7 @@ public:
    * @brief Initialize parameters as needed
    * @param nh NodeHandle to read parameters from
    */
-  virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
 
   /**
    * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h

@@ -25,7 +25,7 @@ public:
    * @brief Initialize parameters as needed
    * @param nh NodeHandle to read parameters from
    */
-  virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
 
   /**
    * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
@@ -55,8 +55,8 @@ public:
 
 
 protected:
-  ros::NodeHandle nh_;
-  ros::NodeHandle nh_kinematics_;
+  robot::NodeHandle nh_;
+  robot::NodeHandle nh_kinematics_;
   geometry_msgs::Pose2D sub_goal_;
 
   double steering_fix_wheel_distance_x_;
@@ -65,7 +65,7 @@ protected:
   double current_direction_;
   double velocity_rotate_min_, velocity_rotate_max_;
   geometry_msgs::Pose2D goal_;
-  ros::Time time_last_cmd_;
+  robot::Time time_last_cmd_;
 
 }; // class RotateToGoal 
 

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

@@ -24,7 +24,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Calculating algorithm

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

@@ -44,7 +44,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, 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
@@ -102,10 +102,9 @@ namespace mkt_algorithm
       virtual void getParams();
 
       /**
-       * @brief Initialize dynamic reconfigure
-       * @param nh NodeHandle to read parameters from
+       * @brief Initialize Kalman filter
        */
-      virtual void initDynamicReconfigure(const robot::NodeHandle &nh);
+      virtual void initKalmanFilter();
 
       /**
        * @brief Dynamically adjust look ahead distance based on the speed

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

@@ -23,7 +23,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, 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

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp

@@ -6,12 +6,12 @@
 namespace  mkt_algorithm
 {
 
-void GoStraight::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+void GoStraight::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
 {
-  nh_       = ros::NodeHandle(nh, name);
+  nh_       = robot::NodeHandle(nh, name);
   name_     = name;
   tf_       = tf;
-  costmap_ros_ = costmap_ros;
+  costmap_robot_ = costmap_robot;
 
   steering_fix_wheel_distance_x_    = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1);
   steering_fix_wheel_distance_y_    = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.);
@@ -19,7 +19,7 @@ void GoStraight::initialize(const ros::NodeHandle & nh, std::string name, TFList
 
 void GoStraight::reset()
 {
-  time_last_cmd_ = ros::Time::now();
+  time_last_cmd_ = robot::Time::now();
 }
 
 bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
@@ -40,7 +40,7 @@ bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::
 nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
 {
   nav_2d_msgs::Twist2DStamped result;
-  result.header.stamp = ros::Time::now();
+  result.header.stamp = robot::Time::now();
 
   geometry_msgs::Pose2D steer_to_baselink_pose;
   

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp

@@ -11,14 +11,14 @@
 namespace  mkt_algorithm
 {
 
-void Bicycle::initialize(const ros::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
 {
-  nh_priv_  = ros::NodeHandle(nh, name);
-  nh_       = ros::NodeHandle("~");
+  nh_priv_  = robot::NodeHandle(nh, name);
+  nh_       = robot::NodeHandle("~");
   name_     = name;
   tf_       = tf;
-  costmap_ros_ = costmap_ros;
-  last_actuator_update_ = ros::Time(0);
+  costmap_robot_ = costmap_robot;
+  last_actuator_update_ = robot::Time(0);
 
   nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
   nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5);
@@ -72,7 +72,7 @@ void Bicycle::initialize(const ros::NodeHandle& nh, std::string name, TFListener
   // Best guess of initial states
   Eigen::VectorXd x0(n);
   x0 << 0.0, 0.0, 0.0;
-  kf_->init(ros::Time::now().toSec(), x0);
+  kf_->init(robot::Time::now().toSec(), x0);
 }
 
 void Bicycle::reset()
@@ -84,7 +84,7 @@ void Bicycle::reset()
   sub_start_index_saved_ = 0;
   x_direction_ = y_direction_= theta_direction_ = 0;
   follow_step_path_ = false;
-  last_actuator_update_ = ros::Time(0);
+  last_actuator_update_ = robot::Time(0);
 }
 
 bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
@@ -122,7 +122,7 @@ bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twi
 nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
 {
   nav_2d_msgs::Twist2DStamped result;
-  result.header.stamp = ros::Time::now();
+  result.header.stamp = robot::Time::now();
   
   geometry_msgs::Pose2D steer_to_baselink_pose;
   
@@ -205,8 +205,8 @@ nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pos
    
     Eigen::VectorXd y(1);
     y << phi_steer;
-    ros::Time current_time = ros::Time::now();
-    double dt = ros::Duration(current_time - last_actuator_update_).toSec();
+    robot::Time current_time = robot::Time::now();
+    double dt = robot::Duration(current_time - last_actuator_update_).toSec();
     last_actuator_update_ = current_time;
     Eigen::MatrixXd A(3, 3); // System dynamics matrix
     A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
@@ -411,8 +411,8 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_
   // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
 
   unsigned int goal_index = start_index;
-  ros::Rate r(30);
-  while (goal_index < last_valid_index && ros::ok())
+  robot::Rate r(30);
+  while (goal_index < last_valid_index && robot::ok())
   {
     // Tìm kiếm vị trí điểm mục tiêu phù hợp
     double S;
@@ -426,7 +426,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_
     bool goal_already_reached = false;
 
     geometry_msgs::PoseArray poseArrayMsg;
-    poseArrayMsg.header.stamp = ros::Time::now();
+    poseArrayMsg.header.stamp = robot::Time::now();
     poseArrayMsg.header.frame_id = frame_id_path_;
 	  // std::stringstream ss;
     for (auto &reached_intermediate_goal : reached_intermediate_goals_)
@@ -516,7 +516,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_
     }
 
     r.sleep();
-    ros::spinOnce();
+    robot::spinOnce();
   }
 
   if (start_index > goal_index)
@@ -569,11 +569,11 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_
   }
 
   // Publish start_index
-  geometry_msgs::PoseStamped pose_st = nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, ros::Time::now());
+  geometry_msgs::PoseStamped pose_st = nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now());
   closet_robot_goal_pub_.publish(pose_st);
 
   // Publish goal_index
-  geometry_msgs::PoseStamped pose_g = nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, ros::Time::now());
+  geometry_msgs::PoseStamped pose_g = nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now());
   current_goal_pub_.publish(pose_g);
  
   return true;
@@ -587,7 +587,7 @@ bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, s
   }
   double x_direction_saved = 0.0;
   geometry_msgs::PoseArray poseArrayMsg;
-  poseArrayMsg.header.stamp = ros::Time::now();
+  poseArrayMsg.header.stamp = robot::Time::now();
   poseArrayMsg.header.frame_id = frame_id_path_;
 
   for (unsigned int i = 1; i < (unsigned int)plan.size(); i++)

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp

@@ -6,12 +6,12 @@
 namespace  mkt_algorithm
 {
 
-void RotateToGoal::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+void RotateToGoal::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
 {
-  nh_       = ros::NodeHandle(nh, name);
+  nh_       = robot::NodeHandle(nh, name);
   name_     = name;
   tf_       = tf;
-  costmap_ros_ = costmap_ros;
+  costmap_robot_ = costmap_robot;
 
   steering_fix_wheel_distance_x_    = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1);
   steering_fix_wheel_distance_y_    = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.);
@@ -22,7 +22,7 @@ void RotateToGoal::initialize(const ros::NodeHandle & nh, std::string name, TFLi
 
 void RotateToGoal::reset()
 {
-  time_last_cmd_ = ros::Time::now();
+  time_last_cmd_ = robot::Time::now();
 }
 
 bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
@@ -41,7 +41,7 @@ bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs
 nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
 {
   nav_2d_msgs::Twist2DStamped result;
-  result.header.stamp = ros::Time::now();
+  result.header.stamp = robot::Time::now();
 
   geometry_msgs::Pose2D steer_to_baselink_pose;
   
@@ -51,9 +51,9 @@ nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D
 
   double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2));
 
-  // ros::Time t = ros::Time::now();
+  // robot::Time t = robot::Time::now();
   // double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec();
-  // time_last_cmd_ = ros::Time::now();
+  // time_last_cmd_ = robot::Time::now();
 
   double angle_rg = angles::shortest_angular_distance(pose.theta, goal_.theta);
   double angle_reduce  = std::min(std::max(fabs(velocity.theta), 0.15), velocity_rotate_max_);

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

@@ -3,7 +3,7 @@
 #include <robot/console.h>
 
 void mkt_algorithm::diff::GoStraight::initialize(
-    robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {

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

@@ -5,7 +5,7 @@
 #define LIMIT_VEL_THETA 0.33
 
 void mkt_algorithm::diff::PredictiveTrajectory::initialize(
-    robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
@@ -16,7 +16,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     traj_ = traj;
     costmap_robot_ = costmap_robot;
     this->getParams();
-    // this->initDynamicReconfigure(nh_priv_);
     nh_.param("publish_topic", enable_publish_, false);
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
@@ -43,51 +42,59 @@ 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;
     }
-
-    // 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);
+    this->initKalmanFilter();
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->initialized_ = true;
-    robot::log_info("[%s:%d] PredictiveTrajectory Initialized successfully", __FILE__, __LINE__);
+    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()
@@ -127,7 +134,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] 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.");
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
@@ -158,7 +165,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::reset()
 {
   if (this->initialized_)
   {
-    robot::log_info("[%s:%d] PredictiveTrajectory is reset", __FILE__, __LINE__);
+    robot::log_info("[%s:%d]\n PredictiveTrajectory is reset", __FILE__, __LINE__);
     reached_intermediate_goals_.clear();
     start_index_saved_vt_.clear();
     this->clear();
@@ -207,7 +214,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
 {
   if (!initialized_)
   {
-    robot::log_error("[%s:%d] This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
+    robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
     return false;
   }
 
@@ -236,7 +243,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
 
   if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
   {
-    robot::log_error("[%s:%d] The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
+    robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
     return false;
   }
   this->getParams();
@@ -248,7 +255,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
   // prune global plan to cut off parts of the past (spatially before the robot)
   if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0))
   {
-    robot::log_error("[%s:%d] pruneGlobalPlan Failed", __FILE__, __LINE__);
+    robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__);
     return false;
   }
 
@@ -269,7 +276,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
     }
     else
     {
-      robot::log_error("[%s:%d] The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution());
+      robot::log_error("[%s:%d]\n The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution());
       return false;
     }
   }
@@ -278,7 +285,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
     unsigned int start_index, goal_index;
     if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_))
     {
-      robot::log_error("[%s:%d] computePlanCommand is Failed", __FILE__, __LINE__);
+      robot::log_error("[%s:%d]\n computePlanCommand is Failed", __FILE__, __LINE__);
       return false;
     }
   }
@@ -287,12 +294,12 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
   transform_plan_.poses.clear();
   if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_))
   {
-    robot::log_warning("[%s:%d] Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
+    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] Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
+  //   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_;
@@ -353,7 +360,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
     }
     catch (std::exception &e)
     {
-      robot::log_error("[%s:%d] getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
+      robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
       return false;
     }
   }
@@ -373,7 +380,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     return result;
   if (compute_plan_.poses.size() < 2)
   {
-    robot::log_warning("[%s:%d] Local compute plan is available", __FILE__, __LINE__);
+    robot::log_warning("[%s:%d]\n Local compute plan is available", __FILE__, __LINE__);
     return result;
   }
 
@@ -395,7 +402,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
-    robot::log_warning("[%s:%d] Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
+    robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
     return result;
   }
 
@@ -404,7 +411,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
 
   if (transformed_plan.poses.empty())
   {
-    robot::log_warning("[%s:%d] Transformed plan is empty after compute lookahead point", __FILE__, __LINE__);
+    robot::log_warning("[%s:%d]\n Transformed plan is empty after compute lookahead point", __FILE__, __LINE__);
     return result;
   }
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
@@ -811,7 +818,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
   }
   catch (const tf3::TransformException &ex)
   {
-    robot::log_debug("[%s:%d] 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", __FILE__, __LINE__, ex.what());
     return false;
   }
   return true;
@@ -840,7 +847,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
   {
     if (global_plan.poses.empty())
     {
-      robot::log_error("[%s:%d] Received plan with zero length", __FILE__, __LINE__);
+      robot::log_error("[%s:%d]\n Received plan with zero length", __FILE__, __LINE__);
       return false;
     }
 
@@ -927,19 +934,19 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
   }
   catch (tf3::LookupException &ex)
   {
-    robot::log_error("[%s:%d] No Transform available Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what());
     return false;
   }
   catch (tf3::ConnectivityException &ex)
   {
-    robot::log_error("[%s:%d] Connectivity Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what());
     return false;
   }
   catch (tf3::ExtrapolationException &ex)
   {
-    robot::log_error("[%s:%d] Extrapolation Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what());
     if (global_plan.poses.size() > 0)
-      robot::log_error("[%s:%d] 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", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
 
     return false;
   }

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

@@ -5,7 +5,7 @@
 #include <angles/angles.h>
 
 void mkt_algorithm::diff::RotateToGoal::initialize(
-    robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
@@ -18,7 +18,7 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
 
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->initialized_ = true;
-    robot::log_info("[%s:%d] RotateToGoal Initialized successfully", __FILE__, __LINE__);
+    robot::log_info("[%s:%d]\n RotateToGoal Initialized successfully", __FILE__, __LINE__);
   }
 }
 
@@ -58,7 +58,7 @@ void mkt_algorithm::diff::RotateToGoal::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] The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
+    robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
   double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);

src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h

@@ -4,10 +4,10 @@
 #ifndef MKT_MSGS_MESSAGE_TRAJECTORY2D_H
 #define MKT_MSGS_MESSAGE_TRAJECTORY2D_H
 
+#include <robot/duration.h>
 #include <string>
 #include <vector>
 #include <memory>
-
 #include <nav_2d_msgs/Twist2D.h>
 #include <geometry_msgs/Pose2D.h>
 

src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt

@@ -0,0 +1,120 @@
+cmake_minimum_required(VERSION 3.10)
+project(mkt_plugins VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+# Build type
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+# Find system dependencies
+find_package(Boost REQUIRED COMPONENTS system)
+
+# Flags biên dịch
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# Thư mục include
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include
+)
+
+# Dependencies packages (internal libraries)
+set(PACKAGES_DIR
+    score_algorithm
+    nav_2d_msgs
+    nav_2d_utils
+    nav_core2
+    geometry_msgs
+    nav_msgs
+    std_msgs
+    sensor_msgs
+    angles
+)
+
+###########
+## Build ##
+###########
+
+# Library 1: goal_checker
+add_library(${PROJECT_NAME}_goal_checker SHARED
+    src/goal_checker.cpp
+    src/simple_goal_checker.cpp
+    src/equation_line.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}_goal_checker
+    PUBLIC ${PACKAGES_DIR}
+    PRIVATE Boost::boost
+)
+
+target_include_directories(${PROJECT_NAME}_goal_checker
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# Library 3: standard_traj_generator
+add_library(${PROJECT_NAME}_standard_traj_generator SHARED
+    src/kinematic_parameters.cpp
+    src/xy_theta_iterator.cpp
+    src/standard_traj_generator.cpp
+    src/limited_accel_generator.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}_standard_traj_generator
+    PUBLIC ${PACKAGES_DIR}
+    PRIVATE Boost::boost
+)
+
+target_include_directories(${PROJECT_NAME}_standard_traj_generator
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+#############
+## Install ##
+#############
+
+# Cài đặt header files
+install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include/${PROJECT_NAME}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+)
+
+# Cài đặt libraries
+install(TARGETS 
+    ${PROJECT_NAME}_standard_traj_generator 
+    ${PROJECT_NAME}_goal_checker
+    EXPORT ${PROJECT_NAME}-targets
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+# Export targets
+install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    DESTINATION lib/cmake/${PROJECT_NAME}
+)
+
+# Tùy chọn build
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
+message(STATUS "=================================")

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

@@ -0,0 +1,74 @@
+#ifndef __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
+#define __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
+#include <robot/node_handle.h>
+#include <geometry_msgs/Pose2D.h>
+#include <iostream>
+
+namespace mkt_plugins
+{
+
+/**
+ * @class NoSolution
+ */
+class NoSolution : public std::exception {
+  public:
+    const char * what () const throw () {
+      return "Robot cannot calculate error line!";
+    }
+}; // class NoSolution
+
+/**
+ * @class LineGenerator
+ */
+class LineGenerator 
+{
+  /* data */
+  struct LineEquation {
+    geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
+    geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
+    float a, b, c ;   /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */  
+  };
+
+  /* Save data present position */
+  struct PresentPosition {
+    float Gxh, Gyh;             /* 𝑮𝒋(𝒙h,𝒚h) The head position in Robot  */ 
+    float Yaw_degree;           /* Angle of Robot (degree)              */
+    float Yaw_rad;              /* Angle of Robot (degree)              */
+    float error_line;           /* 𝒆(𝑮,𝒍𝒊𝒏𝒆)=|𝒂𝒙+𝒃𝒚+𝒄|/sqrt(𝒂^2+𝒃^2)       */
+    double distance_to_goal;    /* The distance from query pose to goal */
+    double distance_to_start;   /* The distance from query pose to start*/
+    double distace_start_goal;
+  };
+public:
+
+  /**
+   * @brief To caculated the equation of the line 
+   * @param start_point   start point of the line
+   * @param end_point     end point of the line
+   */
+  virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
+
+  /**
+   * @brief Calculating error 
+   * @param[in] Lm          The distance from the center of the wheel axis to the center line
+   * @param[in] pose
+   * @param[in] rev         
+   */
+  virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
+
+  /**
+   * @brief Calculating tolerance 
+   * @param query_pose The pose to check
+   * @param goal_pose The pose to check against
+   */
+  virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
+
+  /* Properties */
+  LineEquation      Leq_; 
+  PresentPosition   present_pose_;
+
+}; // class LineGenerator 
+
+} // namespace mkt_plugins
+
+#endif // __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__

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

@@ -0,0 +1,57 @@
+#ifndef _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__
+#define _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__
+
+#include <score_algorithm/goal_checker.h>
+#include <mkt_plugins/equation_line.h>
+
+namespace mkt_plugins
+{
+  class GoalChecker : public score_algorithm::GoalChecker
+  {
+  public:
+    GoalChecker();
+    /**
+     * @brief Initialize any parameters from the NodeHandle
+     * @param nh NodeHandle for grabbing parameters
+     */
+    virtual void initialize(const robot::NodeHandle &nh) override;
+
+    /**
+     * @brief Reset the state of the goal checker (if any)
+     */
+    virtual void reset() override;
+
+    /**
+     * @brief Check whether the goal should be considered reached
+     * @param query_pose The pose to check
+     * @param goal_pose The pose to check against
+     * @param velocity The robot's current velocity
+     * @return True if goal is reached
+     */
+    bool isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose,
+                       const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Factory function to create a GoalChecker instance
+     * @return Shared pointer to a new GoalChecker instance
+     */
+    static score_algorithm::GoalChecker::Ptr create();
+  protected:
+    /**
+     * @brief Internal method to initialize parameters from NodeHandle
+     * @param nh NodeHandle to load parameters from
+     * 
+     * Loads xy_goal_tolerance and yaw_goal_tolerance parameters
+     */
+    void intParam(const robot::NodeHandle &nh);
+
+    robot::NodeHandle nh_;
+    std::shared_ptr<mkt_plugins::LineGenerator> line_generator_;
+    double yaw_goal_tolerance_;
+    double xy_goal_tolerance_;
+    double old_xy_goal_tolerance_;
+  };
+
+} // namespace mkt_plugins
+
+#endif // _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__

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

@@ -0,0 +1,300 @@
+#ifndef MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_
+#define MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_
+
+#include <robot/node_handle.h>
+#include <memory>
+
+namespace mkt_plugins
+{
+
+  /**
+   * @class KinematicParameters
+   * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics
+   */
+  class KinematicParameters
+  {
+  public:
+    KinematicParameters();
+    void initialize(const robot::NodeHandle &nh);
+
+    /**
+     * @brief Get the direction array for X, Y, and theta movement
+     * @return Pointer to an array of integers representing the direction of movement for X, Y, and theta (e.g., 1 for forward/right/clockwise, -1 for backward/left/counterclockwise, 0 for stop)
+     *
+     * This array controls the intended direction of the robot. Sudden changes in the theta component can cause discontinuities in odometry data, potentially leading to localization jumps in AMCL when rotating in place.
+     */
+    int *getDirect();
+
+    /**
+     * @brief Get the minimum linear velocity along the X-axis
+     * @return Minimum linear velocity (m/s) for forward/backward motion
+     *
+     * Used to ensure the robot does not move too slowly along X, which could lead to odometry drift. Less relevant for in-place rotation issues.
+     */
+    double getMinX();
+
+    /**
+     * @brief Get the maximum linear velocity along the X-axis
+     * @return Maximum linear velocity (m/s) for forward/backward motion
+     *
+     * Ensures the robot does not exceed safe speeds along X. Not directly related to rotation-induced localization jumps.
+     */
+    double getMaxX();
+
+    /**
+     * @brief Get the maximum linear acceleration along the X-axis
+     * @return Maximum linear acceleration (m/s²) for forward/backward motion
+     *
+     * High acceleration can cause odometry errors if the robot's hardware cannot keep up. Less relevant for in-place rotation.
+     */
+    double getAccX();
+
+    /**
+     * @brief Get the maximum linear deceleration along the X-axis
+     * @return Maximum linear deceleration (m/s²) for forward/backward motion
+     *
+     * Improper deceleration may cause jerky stops, leading to minor odometry errors. Not a primary cause of rotation-induced localization jumps.
+     */
+    double getDecelX();
+
+    /**
+     * @brief Get the minimum linear velocity along the Y-axis
+     * @return Minimum linear velocity (m/s) for lateral motion (if applicable)
+     *
+     * Typically zero for differential drive robots. Not relevant for in-place rotation.
+     */
+    double getMinY();
+
+    /**
+     * @brief Get the maximum linear velocity along the Y-axis
+     * @return Maximum linear velocity (m/s) for lateral motion
+     *
+     * Typically zero for differential drive robots. Not relevant for in-place rotation.
+     */
+    double getMaxY();
+
+    /**
+     * @brief Get the maximum linear acceleration along the Y-axis
+     * @return Maximum linear acceleration (m/s²) for lateral motion
+     *
+     * Not relevant for in-place rotation in differential drive robots.
+     */
+    double getAccY();
+
+    /**
+     * @brief Get the maximum linear deceleration along the Y-axis
+     * @return Maximum linear deceleration (m/s²) for lateral motion
+     *
+     * Not relevant for in-place rotation in differential drive robots.
+     */
+    double getDecelY();
+
+    /**
+     * @brief Get the minimum combined linear velocity in the XY plane
+     * @return Minimum combined velocity (m/s) for motion in the XY plane
+     *
+     * Ensures the robot moves at a meaningful speed in the XY plane. Not directly related to in-place rotation issues.
+     */
+    double getMinSpeedXY();
+
+    /**
+     * @brief Get the minimum angular velocity for rotation
+     * @return Minimum angular velocity (rad/s) for rotation around the Z-axis
+     *
+     * A very low value may cause the robot to struggle with stable rotation, leading to odometry errors and potential localization jumps in AMCL.
+     */
+    double getMinTheta();
+
+    /**
+     * @brief Get the maximum angular velocity for rotation
+     * @return Maximum angular velocity (rad/s) for rotation around the Z-axis
+     *
+     * A high value (e.g., >2.0 rad/s) can cause wheel slip or odometry errors during fast rotation, leading to localization jumps in AMCL.
+     */
+    double getMaxTheta();
+
+    /**
+     * @brief Get the maximum angular acceleration for rotation
+     * @return Maximum angular acceleration (rad/s²) for rotation around the Z-axis
+     *
+     * High angular acceleration can cause jerky rotation, leading to odometry errors and potential localization jumps in AMCL.
+     */
+    double getAccTheta();
+
+    /**
+     * @brief Get the maximum angular deceleration for rotation
+     * @return Maximum angular deceleration (rad/s²) for rotation around the Z-axis
+     *
+     * Improper deceleration can cause jerky stops during rotation, leading to odometry errors and localization jumps in AMCL.
+     */
+    double getDecelTheta();
+
+    /**
+     * @brief Get the minimum angular speed for rotation
+     * @return Minimum angular speed (rad/s) for rotation around the Z-axis
+     *
+     * Similar to getMinTheta(), a very low value may cause unstable rotation, contributing to odometry errors and AMCL localization issues.
+     */
+    double getMinSpeedTheta();
+
+    /**
+     * @brief Set the direction array for X, Y, and theta movement
+     * @param xytheta_direct Pointer to an array of integers specifying the direction for X, Y, and theta
+     *
+     * Sets the intended direction of motion. Sudden changes in the theta component can cause discontinuities in odometry, leading to localization jumps in AMCL during in-place rotation.
+     */
+    void setDirect(int *xytheta_direct);
+
+    /**
+     * @brief Set the minimum linear velocity along the X-axis
+     * @param value Minimum linear velocity (m/s) for forward/backward motion
+     *
+     * Ensures the robot does not move too slowly along X. Not directly related to in-place rotation issues.
+     */
+    void setMinX(double value);
+
+    /**
+     * @brief Set the maximum linear velocity along the X-axis
+     * @param value Maximum linear velocity (m/s) for forward/backward motion
+     *
+     * Limits the robot's speed along X. Not directly related to in-place rotation issues.
+     */
+    void setMaxX(double value);
+
+    /**
+     * @brief Set the maximum linear acceleration along the X-axis
+     * @param value Maximum linear acceleration (m/s²) for forward/backward motion
+     *
+     * High acceleration can cause odometry errors if not supported by hardware. Less relevant for in-place rotation.
+     */
+    void setAccX(double value);
+
+    /**
+     * @brief Set the maximum linear deceleration along the X-axis
+     * @param value Maximum linear deceleration (m/s²) for forward/backward motion
+     *
+     * Improper deceleration may cause minor odometry errors. Not a primary cause of rotation-induced localization jumps.
+     */
+    void setDecelX(double value);
+
+    /**
+     * @brief Set the minimum linear velocity along the Y-axis
+     * @param value Minimum linear velocity (m/s) for lateral motion
+     *
+     * Typically zero for differential drive robots. Not relevant for in-place rotation.
+     */
+    void setMinY(double value);
+
+    /**
+     * @brief Set the maximum linear velocity along the Y-axis
+     * @param value Maximum linear velocity (m/s) for lateral motion
+     *
+     * Typically zero for differential drive robots. Not relevant for in-place rotation.
+     */
+    void setMaxY(double value);
+
+    /**
+     * @brief Set the maximum linear acceleration along the Y-axis
+     * @param value Maximum linear acceleration (m/s²) for lateral motion
+     *
+     * Not relevant for in-place rotation in differential drive robots.
+     */
+    void setAccY(double value);
+
+    /**
+     * @brief Set the maximum linear deceleration along the Y-axis
+     * @param value Maximum linear deceleration (m/s²) for lateral motion
+     *
+     * Not relevant for in-place rotation in differential drive robots.
+     */
+    void setDecelY(double value);
+
+    /**
+     * @brief Set the minimum combined linear velocity in the XY plane
+     * @param value Minimum combined velocity (m/s) for motion in the XY plane
+     *
+     * Ensures meaningful motion in the XY plane. Not directly related to in-place rotation issues.
+     */
+    void setMinSpeedXY(double value);
+
+    /**
+     * @brief Set the maximum angular velocity for rotation
+     * @param value Maximum angular velocity (rad/s) for rotation around the Z-axis
+     *
+     * A high value can cause fast rotation, leading to wheel slip or odometry errors, which may cause AMCL to jump during in-place rotation.
+     */
+    void setMaxTheta(double value);
+
+    /**
+     * @brief Set the maximum angular acceleration for rotation
+     * @param value Maximum angular acceleration (rad/s²) for rotation around the Z-axis
+     *
+     * High acceleration can cause jerky rotation, leading to odometry errors and AMCL localization jumps.
+     */
+    void setAccTheta(double value);
+
+    /**
+     * @brief Set the maximum angular deceleration for rotation
+     * @param value Maximum angular deceleration (rad/s²) for rotation around the Z-axis
+     *
+     * Improper deceleration can cause jerky stops, leading to odometry errors and AMCL localization jumps.
+     */
+    void setDecelTheta(double value);
+
+    /**
+     * @brief Set the minimum angular speed for rotation
+     * @param value Minimum angular speed (rad/s) for rotation around the Z-axis
+     *
+     * A very low value may cause unstable rotation, contributing to odometry errors and AMCL localization issues.
+     */
+    void setMinSpeedTheta(double value);
+
+    /**
+     * @brief Check to see whether the combined x/y/theta velocities are valid
+     * @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits
+     *
+     * This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta.
+     * The speed is valid if
+     *  1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative)
+     *  AND
+     *  2) min_speed_xy is negative or min_speed_theta is negative or
+     *     hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta.
+     *
+     * In English, it makes sure the diagonal motion is not too fast,
+     * and that the velocity is moving in some meaningful direction.
+     *
+     * In Latin, quod si motus sit signum quaerit et movere ieiunium et significantissime comprehendite.
+     */
+    bool isValidSpeed(double x, double y, double theta);
+
+    using Ptr = std::shared_ptr<KinematicParameters>;
+
+  protected:
+    // For parameter descriptions, see cfg/KinematicParams.cfg
+    int xytheta_direct_[3];
+    double min_vel_x_, min_vel_y_;
+    double max_vel_x_, max_vel_y_, max_vel_theta_;
+
+    double min_speed_xy_, max_speed_xy_;
+    double min_speed_theta_;
+
+    double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
+    double decel_lim_x_, decel_lim_y_, decel_lim_theta_;
+
+    // Cached square values of min_speed_xy and max_speed_xy
+    double min_speed_xy_sq_, max_speed_xy_sq_;
+
+  private:
+    double original_min_vel_x_, original_min_vel_y_;
+    double original_max_vel_x_, original_max_vel_y_, original_max_vel_theta_;
+
+    double original_min_speed_xy_, original_max_speed_xy_;
+    double original_min_speed_theta_;
+
+    double original_acc_lim_x_, original_acc_lim_y_, original_acc_lim_theta_;
+    double original_decel_lim_x_, original_decel_lim_y_, original_decel_lim_theta_;
+  };
+
+} // namespace mkt_plugins
+
+#endif // MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_

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

@@ -0,0 +1,42 @@
+#ifndef MKT_PLUGINS_LIMITED_ACCEL_GENERATOR_H_INCLUDED_
+#define MKT_PLUGINS_LIMITED_ACCEL_GENERATOR_H_INCLUDED_
+
+#include <mkt_plugins/standard_traj_generator.h>
+
+namespace mkt_plugins
+{
+/**
+ * @class LimitedAccelGenerator
+ * @brief Limits the acceleration in the generated trajectories to a fraction of the simulated time.
+ */
+class LimitedAccelGenerator : public StandardTrajectoryGenerator
+{
+public:
+  /**
+   * @brief Initialize the generator with parameters from NodeHandle
+   * @param nh NodeHandle for loading acceleration_time parameter
+   * 
+   * Loads the acceleration_time parameter which limits acceleration to a fraction
+   * of the simulated time.
+   */
+  void initialize(const robot::NodeHandle& nh) override;
+  
+  /**
+   * @brief Start a new iteration with current velocity
+   * @param current_velocity Current velocity of the robot
+   * 
+   * Overrides the base class to apply acceleration limits based on acceleration_time.
+   */
+  void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) override;
+  
+  /**
+   * @brief Factory function to create a LimitedAccelGenerator instance
+   * @return Shared pointer to a new LimitedAccelGenerator instance
+   */
+  static score_algorithm::TrajectoryGenerator::Ptr create();
+protected:
+  double acceleration_time_;
+};
+}  // namespace mkt_plugins
+
+#endif  // MKT_PLUGINS_LIMITED_ACCEL_GENERATOR_H_INCLUDED_

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

@@ -0,0 +1,221 @@
+#ifndef MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED_
+#define MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED_
+
+#include <algorithm>
+#include <cmath>
+#include <robot/node_handle.h>
+
+namespace mkt_plugins
+{
+
+  const double EPSILON = 5E-2;
+
+  inline double exponentialFactor(double x, double k = 2.5)
+  {
+    x = std::clamp(x, 0.0, 1.0);
+    return 1.0 - std::exp(-k * x); // tương đương (1 - e^{-kx})
+  }
+
+  /**
+   * @brief Given initial conditions and a time, figure out the end velocity
+   *
+   * @param v0 Initial velocity
+   * @param accel The acceleration rate
+   * @param decel The decceleration rate
+   * @param dt Delta time - amount of time to project into the future
+   * @param target target velocity
+   * @return The velocity dt seconds after v0.
+   */
+  inline double projectVelocity(double v0, double accel, double decel, double dt, double target)
+  {
+    double magnitude;
+    if (fabs(v0) < EPSILON)
+    {
+      // Starting from standstill, always accelerate
+      if (target >= 0.0)
+      {
+        magnitude = fabs(accel);
+      }
+      else
+      {
+        magnitude = -fabs(accel);
+      }
+    }
+    else if (v0 > 0.0)
+    {
+      if (v0 < target)
+      {
+        // Acceleration (speed magnitude increases)
+        magnitude = fabs(accel);
+      }
+      else
+      {
+        // Deceleration (speed magnitude decreases)
+        magnitude = -fabs(decel);
+      }
+    }
+    else
+    {
+      if (v0 < target)
+      {
+        // Deceleration (speed magnitude decreases)
+        magnitude = fabs(decel);
+      }
+      else
+      {
+        // Acceleration (speed magnitude increases)
+        magnitude = -fabs(accel);
+      }
+    }
+
+    double delta_v = fabs(target - v0);
+    double max_v = std::max(fabs(target), 1e-3);
+    magnitude *= exponentialFactor(delta_v / max_v, 3.0);
+    double v1 = v0 + magnitude * dt;
+
+    if (magnitude > 0.0)
+    {
+      return std::min(target, v1);
+    }
+    else
+    {
+      return std::max(target, v1);
+    }
+  }
+
+  /**
+   * @class OneDVelocityIterator
+   * @brief An iterator for generating a number of samples in a range
+   *
+   * In its simplest usage, this gives us N (num_samples) different velocities that are reachable
+   * given our current velocity. However, there is some fancy logic around zero velocities and
+   * the min/max velocities
+   *
+   * If the current velocity is 2 m/s, and the acceleration limit is 1 m/ss and the acc_time is 1 s,
+   * this class would provide velocities between 1 m/s and 3 m/s.
+   *
+   *
+   *
+   */
+  class OneDVelocityIterator
+  {
+  public:
+    std::string name_;
+    /**
+     * @brief Constructor for the velocity iterator
+     * @param current Current velocity of the robot
+     * @param direct Direction multiplier (1 for forward, -1 for backward, 0 for stop)
+     * @param min Minimum velocity allowable
+     * @param max Maximum velocity allowable
+     * @param acc_limit Maximum acceleration limit
+     * @param decel_limit Maximum deceleration limit
+     * @param acc_time Time delta for velocity projection
+     * @param num_samples Number of velocity samples to generate
+     * 
+     * Creates an iterator that generates num_samples velocities reachable from
+     * 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,
+                         int num_samples)
+    {
+      // if (current < min)
+      // {
+      //   current = min;
+      // }
+      // else if (current > max)
+      // {
+      //   current = max;
+      // }
+      max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max);
+      min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min);
+      direct_ = direct;
+      current_ = direct_ > 0 ? max_vel_ : min_vel_;
+      reset();
+
+      if (min_vel_ - max_vel_ < EPSILON)
+      {
+        increment_ = 1.0;
+        return;
+      }
+      num_samples = std::max(2, num_samples);
+
+      // e.g. for 4 samples, split distance in 3 even parts
+      increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1));
+    }
+
+    /**
+     * @brief Get the current velocity value
+     * @return Current velocity value, or 0.0 if return_zero_now_ is true
+     * 
+     * Returns the velocity at the current iterator position.
+     * If the iterator is set to return zero (when crossing zero velocity),
+     * returns 0.0 instead of the current value.
+     */
+    double getVelocity() const
+    {
+      if (return_zero_now_)
+        return 0.0;
+      return current_;
+    }
+
+    /**
+     * @brief Increment the iterator to the next velocity sample
+     * @return Reference to this iterator
+     * 
+     * Moves to the next velocity sample. If the iterator crosses zero velocity
+     * (from negative to positive), it will return zero velocity on the next call
+     * to getVelocity() before continuing with positive velocities.
+     */
+    OneDVelocityIterator &operator++()
+    {
+      if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 && current_ + increment_ <= max_vel_ + EPSILON)
+      {
+        return_zero_now_ = true;
+        return_zero_ = false;
+      }
+      else
+      {
+        current_ += increment_;
+        return_zero_now_ = false;
+      }
+      return *this;
+    }
+
+    /**
+     * @brief Reset the iterator back to the first velocity
+     * 
+     * Resets the iterator to the starting velocity (max_vel_ if direct > 0,
+     * min_vel_ if direct <= 0) and enables zero-velocity return if applicable.
+     */
+    void reset()
+    {
+      return_zero_ = true;
+      return_zero_now_ = false;
+    }
+
+    /**
+     * @brief Check if all velocities for this iteration have been returned
+     * @return True if the iterator has passed beyond the limit velocity
+     * 
+     * Returns true when the current velocity has exceeded the maximum
+     * (if direct > 0) or minimum (if direct <= 0) velocity limit.
+     */
+    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;
+    }
+
+  private:
+    bool return_zero_, return_zero_now_;
+    double min_vel_, max_vel_;
+    double current_;
+    double direct_;
+    double increment_;
+  };
+} // namespace mkt_plugins
+
+#endif // MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED

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

@@ -0,0 +1,77 @@
+#ifndef _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__
+#define _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__
+
+#include <robot/node_handle.h>
+#include <score_algorithm/goal_checker.h>
+
+namespace mkt_plugins
+{
+
+  /**
+   * @class SimpleGoalChecker
+   * @brief Goal Checker plugin that only checks the position difference
+   *
+   * This class can be stateful if the stateful parameter is set to true (which it is by default).
+   * This means that the goal checker will not check if the xy position matches again once it is found to be true.
+   */
+  class SimpleGoalChecker : public score_algorithm::GoalChecker
+  {
+  public:
+    /**
+     * @brief Default constructor
+     * Initializes default values for goal tolerances and stateful behavior
+     */
+    SimpleGoalChecker();
+    
+    /**
+     * @brief Initialize the goal checker with parameters from NodeHandle
+     * @param nh NodeHandle for loading parameters (xy_goal_tolerance, yaw_goal_tolerance, stateful, check_xy)
+     */
+    void initialize(const robot::NodeHandle &nh) override;
+    
+    /**
+     * @brief Reset the state of the goal checker
+     * Clears any cached state to allow re-checking of goal conditions
+     */
+    void reset() override;
+    
+    /**
+     * @brief Check if the goal has been reached
+     * @param query_pose Current pose of the robot
+     * @param goal_pose Target pose to reach
+     * @param path Path to follow (not used in simple checker)
+     * @param velocity Current velocity of the robot (not used in simple checker)
+     * @return True if the robot is within xy_goal_tolerance and yaw_goal_tolerance of the goal
+     * 
+     * If stateful is true, once xy position matches, it will not check xy again.
+     * This prevents oscillation when the robot is close to the goal.
+     */
+    bool isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose,
+                       const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Factory function to create a SimpleGoalChecker instance
+     * @return Shared pointer to a new SimpleGoalChecker instance
+     */
+    static score_algorithm::GoalChecker::Ptr create();
+
+  protected:
+    /**
+     * @brief Internal method to initialize parameters from NodeHandle
+     * @param nh NodeHandle to load parameters from
+     * 
+     * Loads xy_goal_tolerance, yaw_goal_tolerance, stateful, and check_xy parameters
+     */
+    void intParam(const robot::NodeHandle& nh);
+
+    robot::NodeHandle nh_;
+    double xy_goal_tolerance_, yaw_goal_tolerance_;
+    bool stateful_, check_xy_;
+
+    // Cached squared xy_goal_tolerance_
+    double xy_goal_tolerance_sq_;
+  };
+
+} // namespace mkt_plugins
+
+#endif // _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__

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

@@ -0,0 +1,189 @@
+#ifndef MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_
+#define MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_
+
+#include <robot/node_handle.h>
+#include <score_algorithm/trajectory_generator.h>
+#include <mkt_plugins/velocity_iterator.h>
+#include <mkt_plugins/kinematic_parameters.h>
+#include <vector>
+#include <memory>
+
+namespace mkt_plugins
+{
+  /**
+   * @class StandardTrajectoryGenerator
+   * @brief Standard MKT-like trajectory generator.
+   */
+  class StandardTrajectoryGenerator : public score_algorithm::TrajectoryGenerator
+  {
+  public:
+    /**
+     * @brief Initialize the trajectory generator with parameters from NodeHandle
+     * @param nh NodeHandle for loading configuration parameters
+     * 
+     * Loads parameters such as sim_time, discretize_by_time, time_granularity,
+     * linear_granularity, angular_granularity, and include_last_point.
+     */
+    void initialize(const robot::NodeHandle &nh) override;
+
+    /**
+     * @brief Set the direction array for X, Y, and Theta movement
+     * @param xytheta_direct Pointer to array of 3 integers representing direction
+     *                       (1 for forward/right/clockwise, -1 for backward/left/counterclockwise)
+     */
+    void setDirect(int *xytheta_direct) override;
+
+    /**
+     * @brief set velocity for x, y asix of the robot
+     * @param linear the velocity command
+     * @return True if set is done, false otherwise
+     */
+    bool setTwistLinear(geometry_msgs::Vector3 linear) override;
+
+    /**
+     * @brief get velocity for x, y asix of the robot
+     * @param linear The velocity command
+     * @return True if set is done, false otherwise
+     */
+    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
+
+    /**
+     * @brief Set velocity theta for z asix of the robot
+     * @param angular the command velocity
+     * @return True if set is done, false otherwise
+     */
+    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
+
+    /**
+     * @brief Get velocity theta for z asix of the robot
+     * @param direct The velocity command
+     * @return True if set is done, false otherwise
+     */
+    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
+
+    /**
+     * @brief Start a new iteration with current velocity
+     * @param current_velocity Current velocity of the robot
+     * 
+     * Resets the velocity iterator and prepares it to generate velocities
+     * reachable from the current velocity.
+     */
+    void startNewIteration(const nav_2d_msgs::Twist2D &current_velocity) override;
+    
+    /**
+     * @brief Check if there are more velocity samples available
+     * @return True if there are more valid velocity combinations to iterate
+     */
+    bool hasMoreTwists() override;
+    
+    /**
+     * @brief Get the next velocity sample
+     * @return Next valid velocity combination
+     */
+    nav_2d_msgs::Twist2D nextTwist() override;
+    
+    /**
+     * @brief Generate a trajectory from start pose to goal
+     * @param start_pose Starting pose of the robot
+     * @param transformed_plan Transformed global plan (path to follow)
+     * @param start_vel Starting velocity of the robot
+     * @param cmd_vel Command velocity to achieve
+     * @return Generated trajectory containing poses and velocities over time
+     * 
+     * Generates a trajectory by:
+     * 1. Computing velocity steps based on acceleration limits
+     * 2. Projecting poses forward using kinematic model
+     * 3. Sampling velocities using the velocity iterator
+     */
+    mkt_msgs::Trajectory2D generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose,
+                                              const nav_2d_msgs::Path2D &transformed_plan,
+                                              const nav_2d_msgs::Twist2D &start_vel,
+                                              const nav_2d_msgs::Twist2D &cmd_vel) override;
+
+    /**
+     * @brief Get the NodeHandle used for kinematics configuration
+     * @return NodeHandle scoped to kinematics namespace
+     */
+    virtual robot::NodeHandle getNodeHandle() override;
+
+    /**
+     * @brief Factory function to create a StandardTrajectoryGenerator instance
+     * @return Shared pointer to a new StandardTrajectoryGenerator instance
+     */
+    static score_algorithm::TrajectoryGenerator::Ptr create();
+
+  protected:
+    /**
+     * @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding
+     */
+    virtual void initializeIterator(const robot::NodeHandle &nh);
+
+    /**
+     * @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
+     *
+     * @param cmd_vel Desired velocity
+     * @param start_vel starting velocity
+     * @param dt amount of time in seconds
+     * @return new velocity after dt seconds
+     */
+    virtual nav_2d_msgs::Twist2D computeNewVelocity(const nav_2d_msgs::Twist2D &cmd_vel,
+                                                    const nav_2d_msgs::Twist2D &start_vel,
+                                                    const double dt);
+
+    /**
+     * @brief Use the robot's kinematic model to predict new positions for the robot
+     *
+     * @param start_pose Starting pose
+     * @param vel Actual robot velocity (assumed to be within acceleration limits)
+     * @param dt amount of time in seconds
+     * @return New pose after dt seconds
+     */
+    virtual geometry_msgs::Pose2D computeNewPosition(const geometry_msgs::Pose2D start_pose,
+                                                     const nav_2d_msgs::Twist2D &vel,
+                                                     const double dt);
+
+    /**
+     * @brief Compute an array of time deltas between the points in the generated trajectory.
+     *
+     * @param cmd_vel The desired command velocity
+     * @return vector of the difference between each time step in the generated trajectory
+     *
+     * If we are discretizing by time, the returned vector will be the same constant time_granularity
+     * for all cmd_vels. Otherwise, you will get times based on the linear/angular granularity.
+     *
+     * Right now the vector contains a single value repeated many times, but this method could be overridden
+     * to allow for dynamic spacing
+     */
+    virtual std::vector<double> getTimeSteps(const nav_2d_msgs::Twist2D &cmd_vel);
+
+    robot::NodeHandle nh_kinematics_;
+    KinematicParameters::Ptr kinematics_;
+    std::shared_ptr<mkt_plugins::VelocityIterator> velocity_iterator_;
+
+    double sim_time_;
+
+    // Sampling Parameters
+    bool discretize_by_time_;
+    double time_granularity_;    ///< If discretizing by time, the amount of time between each point in the traj
+    double linear_granularity_;  ///< If not discretizing by time, the amount of linear space between points
+    double angular_granularity_; ///< If not discretizing by time, the amount of angular space between points
+
+    /* Backwards Compatibility Parameter: include_last_point
+     *
+     * dwa had an off-by-one error built into it.
+     * It generated N trajectory points, where N = ceil(sim_time / time_delta).
+     * If for example, sim_time=3.0 and time_delta=1.5, it would generate trajectories with 2 points, which
+     * indeed were time_delta seconds apart. However, the points would be at t=0 and t=1.5, and thus the
+     * actual sim_time was much less than advertised.
+     *
+     * This is remedied by adding one final point at t=sim_time, but only if include_last_point_ is true.
+     *
+     * Nothing I could find actually used the time_delta variable or seemed to care that the trajectories
+     * were not projected out as far as they intended.
+     */
+    bool include_last_point_;
+  }; // class StandardTrajectoryGenerator
+
+} // namespace dwb_plugins
+
+#endif // MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_

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

@@ -0,0 +1,61 @@
+#ifndef MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_
+#define MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_
+
+#include <robot/node_handle.h>
+#include <nav_2d_msgs/Twist2D.h>
+#include <mkt_plugins/kinematic_parameters.h>
+
+namespace mkt_plugins
+{
+/**
+ * @class VelocityIterator
+ * @brief Abstract base class for iterating over valid velocity samples
+ * 
+ * This interface provides a way to generate and iterate through velocity samples
+ * that satisfy kinematic constraints. Implementations should generate velocities
+ * reachable from the current velocity within acceleration limits.
+ */
+class VelocityIterator
+{
+public:
+  /**
+   * @brief Virtual destructor
+   */
+  virtual ~VelocityIterator() {}
+  
+  /**
+   * @brief Initialize the iterator with parameters and kinematics
+   * @param nh NodeHandle for loading configuration parameters
+   * @param kinematics Shared pointer to kinematic parameters for validation
+   */
+  virtual void initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) = 0;
+  
+  /**
+   * @brief Start a new iteration with current velocity and time delta
+   * @param current_velocity Current velocity of the robot
+   * @param dt Time delta for velocity projection
+   * 
+   * Resets the iterator and prepares it to generate velocities reachable
+   * from the current velocity within dt seconds.
+   */
+  virtual void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) = 0;
+  
+  /**
+   * @brief Check if there are more velocity samples available
+   * @return True if there are more valid velocity combinations to iterate
+   */
+  virtual bool hasMoreTwists() = 0;
+  
+  /**
+   * @brief Get the next velocity sample
+   * @return Next valid velocity combination
+   * 
+   * Returns the next velocity sample that satisfies kinematic constraints.
+   * Should only be called when hasMoreTwists() returns true.
+   */
+  virtual nav_2d_msgs::Twist2D nextTwist() = 0;
+};  // class VelocityIterator
+
+}  // namespace mkt_plugins
+
+#endif  // MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_

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

@@ -0,0 +1,79 @@
+#ifndef MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_
+#define MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_
+
+#include <mkt_plugins/velocity_iterator.h>
+#include <mkt_plugins/one_d_velocity_iterator.h>
+#include <memory>
+
+namespace mkt_plugins
+{
+  /**
+   * @class XYThetaIterator
+   * @brief Iterator that generates velocity samples for X, Y, and Theta dimensions
+   * 
+   * This iterator combines three OneDVelocityIterator instances (one for each dimension)
+   * to generate valid velocity combinations that satisfy kinematic constraints.
+   */
+  class XYThetaIterator : public VelocityIterator
+  {
+  public:
+    /**
+     * @brief Default constructor
+     * Initializes all iterators to nullptr
+     */
+    XYThetaIterator() : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
+    
+    /**
+     * @brief Initialize the iterator with parameters and kinematics
+     * @param nh NodeHandle for loading sampling parameters (vx_samples, vy_samples, vtheta_samples)
+     * @param kinematics Shared pointer to kinematic parameters for validation
+     */
+    void initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) override;
+    
+    /**
+     * @brief Start a new iteration with current velocity and time delta
+     * @param current_velocity Current velocity of the robot
+     * @param dt Time delta for velocity projection
+     * 
+     * Creates new OneDVelocityIterator instances for X, Y, and Theta dimensions
+     * based on the current velocity and kinematic constraints.
+     */
+    void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) override;
+    
+    /**
+     * @brief Check if there are more velocity samples to iterate
+     * @return True if there are more valid velocity combinations available
+     */
+    bool hasMoreTwists() override;
+    
+    /**
+     * @brief Get the next velocity sample
+     * @return Next valid velocity combination (x, y, theta)
+     * 
+     * Returns the next valid velocity that satisfies kinematic constraints.
+     * Automatically iterates to the next valid velocity if current is invalid.
+     */
+    nav_2d_msgs::Twist2D nextTwist() override;
+    
+  protected:
+    /**
+     * @brief Check if the current velocity combination is valid
+     * @return True if the current velocity (x_it_, y_it_, th_it_) satisfies kinematic constraints
+     */
+    virtual bool isValidVelocity();
+    
+    /**
+     * @brief Iterate through velocity samples until a valid combination is found
+     * 
+     * Increments the iterators (theta first, then y, then x) until a valid
+     * velocity combination is found or all combinations are exhausted.
+     */
+    void iterateToValidVelocity();
+
+    int vx_samples_, vy_samples_, vtheta_samples_;
+    KinematicParameters::Ptr kinematics_;
+    std::shared_ptr<OneDVelocityIterator> x_it_, y_it_, th_it_;
+  };
+}  // namespace mkt_plugins
+
+#endif  // MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_

src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp

@@ -0,0 +1,118 @@
+#include "mkt_plugins/equation_line.h"
+#include <robot/console.h>
+#include <cmath>
+
+namespace mkt_plugins
+{
+
+bool LineGenerator::calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point)
+{
+  /**
+   *  Phương trình đường thẳng có dạng: 
+   *  (y - yi)/(yj - yi) = (x - xi)/(xj - xi)
+   */
+  Leq_.Gi = start_point; Leq_.Gj = end_point;
+  Leq_.a  = (Leq_.Gj.y - Leq_.Gi.y);
+  Leq_.b  = (Leq_.Gi.x - Leq_.Gj.x);
+  Leq_.c  = (Leq_.Gj.x * Leq_.Gi.y - Leq_.Gi.x * Leq_.Gj.y);
+
+  return Leq_.a != 0 || Leq_.b !=0;
+}
+
+void LineGenerator::calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev)
+{
+  float pha = rev? M_PI : 0;
+  float Yaw_rad   = pose.theta - pha;
+
+/*
+ * Tính tọa độ điểm R
+*/
+  float G_xr       = float(pose.x);
+  float G_yr       = float(pose.y);
+
+/*
+ * Tính tọa độ điểm H
+*/
+  present_pose_.Gxh = G_xr + Lm * cos(Yaw_rad);
+  present_pose_.Gyh = G_yr + Lm * sin(Yaw_rad);
+
+/**
+ *  Phương trình đường thẳng (rh) có dạng: 
+ *  (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
+*/  
+  float A_rh = (present_pose_.Gyh - G_yr);
+  float B_rh = (G_xr - present_pose_.Gxh);
+  float C_rh = (present_pose_.Gxh * G_yr - G_xr * present_pose_.Gyh);
+/*
+ * Phương trình đường thẳng vuông góc (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
+ * và đi qua Gh có dạng: 
+ * -B_rh*(x - Gxh) + Arh*(y - Gyh) = 0
+*/
+  float A_he = -B_rh;
+  float B_he = A_rh;
+  float C_he = (-B_rh) * (-present_pose_.Gxh) + A_rh * (-present_pose_.Gyh);
+/*
+ * Đường thẳng (he) giao với đường thẳng (ij) tại e 
+ * vậy G_xe và G_ye là nghiệm của hệ phương trình 2 ẩn của 2 đường thẳng trên:
+*/
+  float D = A_he * Leq_.b -  Leq_.a * B_he;
+  float Dx = (-C_he) * Leq_.b - (-Leq_.c) * B_he;
+  float Dy = A_he * (-Leq_.c) - Leq_.a * (-C_he);
+/*
+ * Tính tọa độ điểm E
+*/
+  if(D == 0) {
+    if(Dx + Dy == 0) 
+    {
+/*
+ * Vì hai đường thẳng trùng nhau nên sai số giữa robot và đường đi là bằng 0
+*/
+      present_pose_.error_line = 0;
+    }
+    else throw NoSolution();
+  }
+  else 
+  {
+    float G_xe = Dx/D;
+    float G_ye = Dy/D;
+/*
+ * Tính khoảng cách từ điểm E đến đường thẳng (RH)
+*/
+    present_pose_.error_line = -(A_rh * G_xe + B_rh * G_ye + C_rh) / (sqrt(pow(A_rh, 2) + pow(B_rh, 2)));
+  }
+  // present_pose_.error_line        = (Leq.a * present_pose_.Gxh + Leq.b * present_pose_.Gyh + Leq.c) / (sqrt(pow(Leq.a, 2) + pow(Leq.b, 2)));
+  present_pose_.distance_to_goal      = sqrt(pow((Leq_.Gj.x - G_xr),2) + pow((Leq_.Gj.y - G_yr),2));  
+  present_pose_.distance_to_start    = sqrt(pow((Leq_.Gi.x - G_xr),2) + pow((Leq_.Gi.y - G_yr),2));
+
+ 
+}
+
+double LineGenerator::calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose)
+{
+  /**
+   *  Phương trình đường thẳng có dạng: 
+   *  (y - yi)/(yj - yi) = (x - xi)/(xj - xi)
+   */
+  // LineEquation Leq;
+  // Leq.Gi = query_pose; Leq.Gj = goal_pose;
+  // Leq.a  = (Leq.Gj.y - Leq.Gi.y);
+  // Leq.b  = (Leq.Gi.x - Leq.Gj.x);
+  // Leq.c  = (Leq.Gj.x * Leq.Gi.y - Leq.Gi.x * Leq.Gj.y);
+
+  /**
+   * Phương trình đường thẳng vuông góc
+  */
+  LineEquation Leq_l;
+  Leq_l.a = -Leq_.b;
+  Leq_l.b = Leq_.a;
+  Leq_l.c = Leq_.b * goal_pose.x - Leq_.a * goal_pose.y;
+
+  present_pose_.distance_to_goal     = sqrt(pow((Leq_.Gj.x - query_pose.x),2) + pow((Leq_.Gj.y - query_pose.y),2));  
+  present_pose_.distance_to_start    = sqrt(pow((Leq_.Gi.x - query_pose.x),2) + pow((Leq_.Gi.y - query_pose.y),2));
+  present_pose_.distace_start_goal   = sqrt(pow((Leq_.Gj.x - Leq_.Gi.x),2) + pow((Leq_.Gj.y - Leq_.Gi.y),2)); 
+  // ROS_INFO_THROTTLE(0.2, "Goal checker %f %f %f %f %f", Leq_.Gj.x, Leq_.Gj.y, Leq_.Gj.theta, query_pose.x, query_pose.y);
+ 
+  return (Leq_l.a * query_pose.x + Leq_l.b * query_pose.y + Leq_l.c ) / (sqrt(pow(Leq_l.a, 2) + pow(Leq_l.b, 2)));
+}
+
+} // namespace mkt_plugins

src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp

@@ -0,0 +1,106 @@
+#include <robot/console.h>
+#include <mkt_plugins/goal_checker.h>
+#include <boost/dll/alias.hpp>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/path_ops.h>
+#include <nav_2d_utils/parameters.h>
+#include <angles/angles.h>
+#include <cmath>
+
+mkt_plugins::GoalChecker::GoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25)
+{
+}
+
+void mkt_plugins::GoalChecker::initialize(const robot::NodeHandle &nh)
+{
+  nh_ = nh;
+  this->intParam(nh);
+  robot::log_info_at(__FILE__, __LINE__, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f", xy_goal_tolerance_, yaw_goal_tolerance_);
+  line_generator_ = std::make_shared<mkt_plugins::LineGenerator>();
+}
+
+void mkt_plugins::GoalChecker::intParam(const robot::NodeHandle &nh)
+{
+  if (!nh.param<double>("xy_goal_tolerance", xy_goal_tolerance_, 0.1))
+  {
+    xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1);
+  }
+  if (!nh.param<double>("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1))
+  {
+    yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
+  }
+  // ROS_INFO_THROTTLE(0.5, "%s", nh_.getNamespace().c_str());
+  // ROS_INFO_THROTTLE(0.5, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_);
+}
+
+void mkt_plugins::GoalChecker::reset()
+{
+  old_xy_goal_tolerance_ = 0;
+}
+
+bool mkt_plugins::GoalChecker::isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose,
+                                             const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity)
+{
+  this->intParam(nh_);
+  double dx = query_pose.pose.x - goal_pose.pose.x;
+  double dy = query_pose.pose.y - goal_pose.pose.y;
+  double xy_tolerance = std::abs(std::sqrt(dx * dx + dy * dy));
+
+  if (!path.poses.empty() && path.poses.size() > 2)
+  {
+    double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta);
+    double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy;
+    double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy;
+    if (xy_tolerance <= xy_goal_tolerance_ + 0.3)
+    {
+      double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
+      // ROS_INFO_THROTTLE(0.1, "Goal checker 1 %f %f %f %f %f %f", tolerance, old_xy_goal_tolerance_, goal_pose.pose.theta, x, y, theta);
+      if(
+          (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) 
+          // && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4)
+        )
+      {
+        robot::log_info_throttle(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
+        robot::log_info_throttle(0.1, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+        return true;
+      }
+    }
+    else
+      old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
+  }
+  else
+  {
+    double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta);
+    double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy;
+    double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy;
+    if (xy_tolerance <= xy_goal_tolerance_ + 0.3)
+    {
+      double tolerance = fabs(x) > fabs(y) ? x : y;
+      if(
+          (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) 
+          // && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4)
+        )
+      {
+        robot::log_info_throttle(0.1, "%f %f", fabs(cos(theta)), fabs(sin(theta)));
+        robot::log_info_throttle(0.1, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+        return true;
+      }
+    }
+    else
+      old_xy_goal_tolerance_ = fabs(x) > fabs(y) ? x : y;
+  }
+
+  if (xy_tolerance <= xy_goal_tolerance_)
+  {
+    robot::log_info_at(__FILE__, __LINE__, "Goal checker 0 %f %f", xy_tolerance, xy_goal_tolerance_);
+    return true;
+  }
+  return false;
+}
+
+score_algorithm::GoalChecker::Ptr mkt_plugins::GoalChecker::create()
+{
+  return std::make_shared<mkt_plugins::GoalChecker>();
+}
+
+BOOST_DLL_ALIAS(mkt_plugins::GoalChecker::create, GoalChecker)

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

@@ -0,0 +1,178 @@
+#include <mkt_plugins/kinematic_parameters.h>
+#include <nav_2d_utils/parameters.h>
+#include <memory>
+#include <string>
+#include <cmath>
+
+
+using nav_2d_utils::moveDeprecatedParameter;
+
+namespace mkt_plugins
+{
+
+  const double EPSILON = 1E-5;
+
+  /**
+   * @brief Helper function to set the deceleration to the negative acceleration if it was not already set.
+   * @param nh NodeHandle
+   * @param dimension String representing the dimension, used in constructing parameter names
+   */
+  void setDecelerationAsNeeded(const robot::NodeHandle &nh, const std::string dimension)
+  {
+    std::string decel_param = "decel_lim_" + dimension;
+    if (nh.hasParam(decel_param))
+      return;
+
+    std::string accel_param = "acc_lim_" + dimension;
+    if (!nh.hasParam(accel_param))
+      return;
+
+    double accel;
+    nh.getParam(accel_param, accel);
+    nh.setParam(decel_param, -accel);
+  }
+
+  KinematicParameters::KinematicParameters() : xytheta_direct_(),
+                                               min_vel_x_(0.0), min_vel_y_(0.0), max_vel_x_(0.0), max_vel_y_(0.0), max_vel_theta_(0.0),
+                                               min_speed_xy_(0.0), max_speed_xy_(0.0), min_speed_theta_(0.0),
+                                               acc_lim_x_(0.0), acc_lim_y_(0.0), acc_lim_theta_(0.0),
+                                               decel_lim_x_(0.0), decel_lim_y_(0.0), decel_lim_theta_(0.0),
+                                               min_speed_xy_sq_(0.0), max_speed_xy_sq_(0.0)
+  {
+  }
+
+  void KinematicParameters::initialize(const robot::NodeHandle &nh)
+  {
+    // Special handling for renamed parameters
+    moveDeprecatedParameter<double>(nh, "max_vel_theta", "max_rot_vel");
+    moveDeprecatedParameter<double>(nh, "min_speed_xy", "min_trans_vel");
+    moveDeprecatedParameter<double>(nh, "max_speed_xy", "max_trans_vel");
+    moveDeprecatedParameter<double>(nh, "min_speed_theta", "min_rot_vel");
+
+    // Set the deceleration parameters to negative the acceleration if the deceleration not already specified
+    setDecelerationAsNeeded(nh, "x");
+    setDecelerationAsNeeded(nh, "y");
+    setDecelerationAsNeeded(nh, "theta");
+
+
+    xytheta_direct_[0] = xytheta_direct_[1] = xytheta_direct_[2] = 1;
+  }
+
+  void KinematicParameters::setDirect(int *xytheta_direct)
+  {
+    xytheta_direct_[0] = xytheta_direct[0];
+    xytheta_direct_[1] = xytheta_direct[1];
+    xytheta_direct_[2] = xytheta_direct[2];
+  }
+
+  bool KinematicParameters::isValidSpeed(double x, double y, double theta)
+  {
+    double vmag_sq = x * x + y * y;
+    if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON)
+      return false;
+    if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ &&
+        min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_)
+      return false;
+    if (vmag_sq == 0.0 && theta == 0.0)
+      return false;
+    return true;
+  }
+
+  int *KinematicParameters::getDirect() { return xytheta_direct_; }
+
+  double KinematicParameters::getMinX() { return min_vel_x_; }
+
+  double KinematicParameters::getMaxX() { return max_vel_x_; }
+
+  double KinematicParameters::getAccX() { return acc_lim_x_; }
+
+  double KinematicParameters::getDecelX() { return decel_lim_x_; }
+
+  double KinematicParameters::getMinY() { return min_vel_y_; }
+
+  double KinematicParameters::getMaxY() { return max_vel_y_; }
+
+  double KinematicParameters::getAccY() { return acc_lim_y_; }
+
+  double KinematicParameters::getDecelY() { return decel_lim_y_; }
+
+  double KinematicParameters::getMinSpeedXY() { return min_speed_xy_; }
+
+  double KinematicParameters::getMinTheta() { return -max_vel_theta_; }
+
+  double KinematicParameters::getMaxTheta() { return max_vel_theta_; }
+
+  double KinematicParameters::getAccTheta() { return acc_lim_theta_; }
+
+  double KinematicParameters::getDecelTheta() { return decel_lim_theta_; }
+
+  double KinematicParameters::getMinSpeedTheta() { return min_speed_theta_; }
+
+  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)
+  {
+    acc_lim_x_ = fabs(value) <= fabs(original_acc_lim_x_) + EPSILON ? value : acc_lim_x_;
+  }
+
+  void KinematicParameters::setDecelX(double value)
+  {
+    decel_lim_x_ = fabs(value) <= fabs(original_decel_lim_x_) + EPSILON ? value : decel_lim_x_;
+  }
+
+  void KinematicParameters::setMinY(double value)
+  {
+    min_vel_y_ = fabs(value) <= fabs(original_min_vel_y_) + EPSILON ? value : min_vel_y_;
+  }
+
+  void KinematicParameters::setMaxY(double value)
+  {
+    max_vel_y_ = fabs(value) <= fabs(original_max_vel_y_) + EPSILON ? value : max_vel_y_;
+  }
+
+  void KinematicParameters::setAccY(double value)
+  {
+    acc_lim_y_ = fabs(value) <= fabs(original_acc_lim_y_) + EPSILON ? value : acc_lim_y_;
+  }
+
+  void KinematicParameters::setDecelY(double value)
+  {
+    decel_lim_y_ = fabs(value) <= fabs(original_decel_lim_y_) + EPSILON ? value : decel_lim_y_;
+  }
+
+  void KinematicParameters::setMinSpeedXY(double value)
+  {
+    min_speed_xy_ = fabs(value) <= fabs(original_min_speed_xy_) + EPSILON ? value : min_speed_xy_;
+  }
+
+  void KinematicParameters::setMaxTheta(double value)
+  {
+    max_vel_theta_  = fabs(value) <= fabs(original_max_vel_theta_) + EPSILON? value : original_max_vel_theta_;
+    // ROS_WARN_THROTTLE(10, "vtheta %f %f %f", value, original_max_vel_theta_, max_vel_theta_);
+  }
+
+  void KinematicParameters::setAccTheta(double value)
+  {
+    acc_lim_theta_ = fabs(value) <= fabs(original_acc_lim_theta_) + EPSILON ? value : acc_lim_theta_;
+  }
+
+  void KinematicParameters::setDecelTheta(double value)
+  {
+    decel_lim_theta_ = fabs(value) <= fabs(original_decel_lim_theta_) + EPSILON ? value : decel_lim_theta_;
+  }
+
+  void KinematicParameters::setMinSpeedTheta(double value) 
+  { 
+    min_speed_theta_ = fabs(value) <= fabs(original_min_speed_theta_) + EPSILON ? value : min_speed_theta_;
+  }
+} // namespace mkt_plugins

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

@@ -0,0 +1,47 @@
+#include <robot/console.h>
+#include <mkt_plugins/limited_accel_generator.h>
+#include <nav_2d_utils/parameters.h>
+#include <boost/dll/alias.hpp>
+#include <nav_core2/exceptions.h>
+#include <vector>
+
+namespace mkt_plugins
+{
+
+void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
+{
+  StandardTrajectoryGenerator::initialize(nh);
+  if (nh.hasParam("sim_period"))
+  {
+    nh.getParam("sim_period", acceleration_time_);
+  }
+  else
+  {
+    double controller_frequency = nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0);
+    if (controller_frequency > 0)
+    {
+      acceleration_time_ = 1.0 / controller_frequency;
+    }
+    else
+    {
+      robot::log_warning_at(__FILE__, __LINE__, "A controller_frequency less than or equal to 0 has been set. "
+                                              "Ignoring the parameter, assuming a rate of 20Hz");
+      acceleration_time_ = 0.05;
+    }
+  }
+}
+
+void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity)
+{
+  // Limit our search space to just those within the limited acceleration_time
+  velocity_iterator_->startNewIteration(current_velocity, acceleration_time_);
+}
+
+  score_algorithm::TrajectoryGenerator::Ptr LimitedAccelGenerator::create()
+  {
+    return std::make_shared<LimitedAccelGenerator>();
+  }
+
+}  // namespace mkt_plugins
+
+BOOST_DLL_ALIAS(mkt_plugins::LimitedAccelGenerator::create, LimitedAccelGenerator)

src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp

@@ -0,0 +1,76 @@
+#include <mkt_plugins/simple_goal_checker.h>
+#include <boost/dll/alias.hpp>
+#include <angles/angles.h>
+#include <nav_2d_utils/parameters.h>
+#include <cmath>
+
+namespace mkt_plugins
+{
+
+SimpleGoalChecker::SimpleGoalChecker() :
+  xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625)
+{
+}
+
+void SimpleGoalChecker::initialize(const robot::NodeHandle& nh)
+{
+  nh_ = nh;
+  this->intParam(nh);
+  robot::log_info_at(__FILE__, __LINE__, "[SimpleGoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f stateful %x check_xy_ %x" , xy_goal_tolerance_, yaw_goal_tolerance_, stateful_, check_xy_);
+}
+
+void SimpleGoalChecker::intParam(const robot::NodeHandle& nh)
+{
+  if(!nh.param<double>("xy_goal_tolerance", xy_goal_tolerance_, 0.1))
+  {
+    xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1);
+  }
+  if(!nh.param<double>("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1))
+  {
+    yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
+  }
+  if(nh.param<bool>("stateful", stateful_, true))
+  {
+    stateful_ = nav_2d_utils::searchAndGetParam(nh, "stateful", true);
+  }
+  robot::log_info_throttle(1.0,"[SimpleGoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f stateful %x check_xy_ %x" , xy_goal_tolerance_, yaw_goal_tolerance_, stateful_, check_xy_);
+  xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
+}
+
+void SimpleGoalChecker::reset()
+{
+  check_xy_ = true;
+}
+
+bool SimpleGoalChecker::isGoalReached(const nav_2d_msgs::Pose2DStamped& query_pose, const nav_2d_msgs::Pose2DStamped& goal_pose,
+                                      const nav_2d_msgs::Path2D& path, const nav_2d_msgs::Twist2D& velocity)
+{
+  this->intParam(nh_);
+  if (check_xy_)
+  {
+    double dx = query_pose.pose.x - goal_pose.pose.x,
+           dy = query_pose.pose.y - goal_pose.pose.y;
+    if (dx * dx + dy * dy > 0.25)
+    {
+      return false;
+    }
+    // We are within the window
+    // If we are stateful, change the state.
+    if (stateful_)
+    {
+      check_xy_ = false;
+    }
+  }
+  double dyaw = angles::shortest_angular_distance(query_pose.pose.theta, goal_pose.pose.theta);
+  // robot::log_info_throttle(1.0, "dyaw %f yaw_goal_tolerance_ %f", fabs(dyaw), yaw_goal_tolerance_);
+  return fabs(dyaw) < yaw_goal_tolerance_;
+}
+
+}  // namespace mkt_plugins
+
+score_algorithm::GoalChecker::Ptr mkt_plugins::SimpleGoalChecker::create()
+{
+  return std::make_shared<mkt_plugins::SimpleGoalChecker>();
+}
+
+BOOST_DLL_ALIAS(mkt_plugins::SimpleGoalChecker::create, SimpleGoalChecker)

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

@@ -0,0 +1,264 @@
+#include <mkt_plugins/standard_traj_generator.h>
+#include <mkt_plugins/xy_theta_iterator.h>
+#include <nav_2d_utils/parameters.h>
+#include <boost/dll/alias.hpp>
+#include <nav_core2/exceptions.h>
+#include <algorithm>
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+
+using nav_2d_utils::loadParameterWithDeprecation;
+
+namespace mkt_plugins
+{
+  void StandardTrajectoryGenerator::initialize(const robot::NodeHandle &nh)
+  {
+    nh_kinematics_ = nh;
+    kinematics_ = std::make_shared<mkt_plugins::KinematicParameters>();
+    kinematics_->initialize(nh);
+    initializeIterator(nh);
+
+    nh.param("sim_time", sim_time_, 1.7);
+
+    nh.param("include_last_point", include_last_point_, true);
+    /*
+     * If discretize_by_time, then sim_granularity represents the amount of time that should be between
+     *  two successive points on the trajectory.
+     *
+     * If discretize_by_time is false, then sim_granularity is the maximum amount of distance between
+     *  two successive points on the trajectory, and angular_sim_granularity is the maximum amount of
+     *  angular distance between two successive points.
+     */
+    nh.param("discretize_by_time", discretize_by_time_, false);
+    if (discretize_by_time_)
+    {
+      time_granularity_ = loadParameterWithDeprecation(nh, "time_granularity", "sim_granularity", 0.025);
+    }
+    else
+    {
+      linear_granularity_ = loadParameterWithDeprecation(nh, "linear_granularity", "sim_granularity", 0.025);
+      angular_granularity_ = loadParameterWithDeprecation(nh, "angular_granularity", "angular_sim_granularity", 0.1);
+    }
+  }
+
+  robot::NodeHandle StandardTrajectoryGenerator::getNodeHandle()
+  {
+    return nh_kinematics_;
+  }
+
+  void StandardTrajectoryGenerator::initializeIterator(const robot::NodeHandle &nh)
+  {
+    velocity_iterator_ = std::make_shared<mkt_plugins::XYThetaIterator>();
+    velocity_iterator_->initialize(nh, kinematics_);
+  }
+
+  void StandardTrajectoryGenerator::setDirect(int *direct)
+  {
+    kinematics_->setDirect(direct);
+  }
+
+  bool StandardTrajectoryGenerator::setTwistLinear(geometry_msgs::Vector3 linear)
+  {
+    try
+    {
+      // ROS_INFO("Get linear %f ", linear.x);
+      if (linear.x > 0)
+        kinematics_->setMaxX(linear.x);
+      else if (linear.x < 0)
+        kinematics_->setMinX(linear.x);
+      else
+      {
+        kinematics_->setMinX(linear.x);
+        kinematics_->setMaxX(linear.x);
+      }
+
+      if (linear.y > 0)
+        kinematics_->setMaxY(linear.y);
+      else if (linear.y < 0)
+        kinematics_->setMinY(linear.y);
+      else
+      {
+        kinematics_->setMinY(linear.y);
+        kinematics_->setMaxY(linear.y);
+      }
+    }
+    catch (const std::exception &e)
+    {
+      throw std::exception(e);
+      return false;
+    }
+
+    return true;
+  }
+
+  geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistLinear(bool direct)
+  {
+    geometry_msgs::Vector3 linear;
+    try
+    {
+      if (direct)
+      {
+        linear.x = kinematics_->getMaxX();
+        linear.y = kinematics_->getMaxY();
+      }
+      else
+      {
+        linear.x = kinematics_->getMinX();
+        linear.y = kinematics_->getMinY();
+      }
+    }
+    catch (const std::exception &e)
+    {
+      throw std::exception(e);
+    }
+    return linear;
+  }
+
+  bool StandardTrajectoryGenerator::setTwistAngular(geometry_msgs::Vector3 angular)
+  {
+    try
+    {
+      kinematics_->setMaxTheta(angular.z);
+    }
+    catch (const std::exception &e)
+    {
+      throw std::exception(e);
+      return false;
+    }
+    return true;
+  }
+
+  geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistAngular(bool direct)
+  {
+    geometry_msgs::Vector3 angular;
+    try
+    {
+      if (direct)
+      {
+        angular.z = kinematics_->getMaxTheta();
+      }
+      else
+      {
+        angular.z = kinematics_->getMinTheta();
+      }
+    }
+    catch (const std::exception &e)
+    {
+      throw std::exception(e);
+    }
+    return angular;
+  }
+
+  void StandardTrajectoryGenerator::startNewIteration(const nav_2d_msgs::Twist2D &current_velocity)
+  {
+    velocity_iterator_->startNewIteration(current_velocity, sim_time_);
+  }
+
+  bool StandardTrajectoryGenerator::hasMoreTwists()
+  {
+    return velocity_iterator_->hasMoreTwists();
+  }
+
+  nav_2d_msgs::Twist2D StandardTrajectoryGenerator::nextTwist()
+  {
+    return velocity_iterator_->nextTwist();
+  }
+
+  std::vector<double> StandardTrajectoryGenerator::getTimeSteps(const nav_2d_msgs::Twist2D &cmd_vel)
+  {
+    std::vector<double> steps;
+    if (discretize_by_time_)
+    {
+      steps.resize(ceil(sim_time_ / time_granularity_));
+    }
+    else // discretize by distance
+    {
+      double vmag = hypot(cmd_vel.x, cmd_vel.y);
+
+      // the distance the robot would travel in sim_time if it did not change velocity
+      double projected_linear_distance = vmag * sim_time_;
+
+      // the angle the robot would rotate in sim_time
+      double projected_angular_distance = fabs(cmd_vel.theta) * sim_time_;
+
+      // Pick the maximum of the two
+      int num_steps = ceil(std::max(projected_linear_distance / linear_granularity_,
+                                    projected_angular_distance / angular_granularity_));
+      steps.resize(num_steps);
+    }
+    if (steps.size() == 0)
+    {
+      steps.resize(1);
+    }
+    std::fill(steps.begin(), steps.end(), sim_time_ / steps.size());
+    return steps;
+  }
+
+  mkt_msgs::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose,
+                                                                         const nav_2d_msgs::Path2D &transformed_plan,
+                                                                         const nav_2d_msgs::Twist2D &start_vel,
+                                                                         const nav_2d_msgs::Twist2D &cmd_vel)
+  {
+    mkt_msgs::Trajectory2D traj;
+    traj.velocity = cmd_vel;
+
+    //  simulate the trajectory
+    geometry_msgs::Pose2D pose = start_pose.pose;
+    nav_2d_msgs::Twist2D vel = start_vel;
+    double running_time = 0.0;
+    std::vector<double> steps = getTimeSteps(cmd_vel);
+
+    for (double dt : steps)
+    {
+      traj.poses.push_back(pose);
+      traj.time_offsets.push_back(robot::Duration(running_time));
+      //  calculate velocities
+      vel = computeNewVelocity(cmd_vel, vel, dt);
+
+      //  update the position of the robot using the velocities passed in
+      pose = computeNewPosition(pose, vel, dt);
+      running_time += dt;
+    } //  end for simulation steps
+
+    if (include_last_point_)
+    {
+      traj.poses.push_back(pose);
+      traj.time_offsets.push_back(robot::Duration(running_time));
+    }
+
+    return traj;
+  }
+
+  /**
+   * change vel using acceleration limits to converge towards sample_target-vel
+   */
+  nav_2d_msgs::Twist2D StandardTrajectoryGenerator::computeNewVelocity(const nav_2d_msgs::Twist2D &cmd_vel,
+                                                                       const nav_2d_msgs::Twist2D &start_vel, const double dt)
+  {
+    nav_2d_msgs::Twist2D new_vel;
+    new_vel.x = projectVelocity(start_vel.x, kinematics_->getAccX(), kinematics_->getDecelX(), dt, cmd_vel.x);
+    new_vel.y = projectVelocity(start_vel.y, kinematics_->getAccY(), kinematics_->getDecelY(), dt, cmd_vel.y);
+    new_vel.theta = projectVelocity(start_vel.theta, kinematics_->getAccTheta(), kinematics_->getDecelTheta(),
+                                    dt, cmd_vel.theta);
+    return new_vel;
+  }
+
+  geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const geometry_msgs::Pose2D start_pose,
+                                                                        const nav_2d_msgs::Twist2D &vel, const double dt)
+  {
+    geometry_msgs::Pose2D new_pose;
+    new_pose.x = start_pose.x + (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
+    new_pose.y = start_pose.y + (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
+    new_pose.theta = start_pose.theta + vel.theta * dt;
+    return new_pose;
+  }
+
+  score_algorithm::TrajectoryGenerator::Ptr StandardTrajectoryGenerator::create()
+  {
+    return std::make_shared<StandardTrajectoryGenerator>();
+  }
+} // namespace mkt_plugins
+
+BOOST_DLL_ALIAS(mkt_plugins::StandardTrajectoryGenerator::create, StandardTrajectoryGenerator)

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

@@ -0,0 +1,73 @@
+#include <mkt_plugins/xy_theta_iterator.h>
+#include <nav_2d_utils/parameters.h>
+#include <memory>
+
+namespace mkt_plugins
+{
+void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) 
+{
+  kinematics_ = kinematics;
+  nh.param("vx_samples", vx_samples_, 20);
+  nh.param("vy_samples", vy_samples_, 5);
+  vtheta_samples_ = nav_2d_utils::loadParameterWithDeprecation(nh, "vtheta_samples", "vth_samples", 20);
+}
+
+void XYThetaIterator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt)
+{
+  x_it_ = std::make_shared<OneDVelocityIterator>(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(),
+                                                 kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_);
+
+  y_it_->name_ = std::string("y_it_");
+  
+  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()
+{
+  return x_it_ && !x_it_->isFinished();
+}
+
+nav_2d_msgs::Twist2D XYThetaIterator::nextTwist()
+{
+  nav_2d_msgs::Twist2D velocity;
+  velocity.x = x_it_->getVelocity();
+  velocity.y = y_it_->getVelocity();
+  velocity.theta = th_it_->getVelocity();
+  iterateToValidVelocity();
+  return velocity;
+}
+
+bool XYThetaIterator::isValidVelocity()
+{
+  return kinematics_->isValidSpeed(x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity());
+}
+
+void XYThetaIterator::iterateToValidVelocity()
+{
+  bool valid = false;
+  while (!valid && hasMoreTwists())
+  {
+    ++(*th_it_);
+    if (th_it_->isFinished())
+    {
+      th_it_->reset();
+      ++(*y_it_);
+      if (y_it_->isFinished())
+      {
+        y_it_->reset();
+        ++(*x_it_);
+      }
+    }
+    valid = isValidVelocity();
+  }
+}
+
+} // namespace mkt_plugins
+

src/Algorithms/Packages/global_planners/dock_planner

@@ -0,0 +1 @@
+Subproject commit c5c55147b754158e16278e73710ebbee76e6290c

src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt

@@ -42,7 +42,7 @@ add_library(two_points_planner SHARED ${SOURCES} ${HEADERS})
 # Link libraries
 target_link_libraries(two_points_planner
     PUBLIC ${PACKAGES_DIR}
-    PUBLIC robot::node_handle robot::console
+    PUBLIC robot_cpp
     PRIVATE Boost::boost
 )
 

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -1,3 +1,4 @@
+#include <robot/console.h>
 #include <two_points_planner/two_points_planner.h>
 #include <costmap_2d/inflation_layer.h>
 #include <boost/dll/alias.hpp>
@@ -25,7 +26,7 @@ namespace two_points_planner
     if (!initialized_)
     {
       robot::NodeHandle nh_priv_("~/" + name);
-      printf("TwoPointsPlanner: Name is %s\n", name.c_str());
+      robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());
 
       int lethal_obstacle;
       nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
@@ -37,17 +38,16 @@ namespace two_points_planner
       name_ = name;
       costmap_robot_ = costmap_robot;
       
-      // Bug
       if(!costmap_robot_ || !costmap_robot_->getCostmap()) 
       {
-        robot::printf_yellow("[%s:%d]TwoPointsPlanner Initialized failed\n", __FILE__, __LINE__);
+        robot::log_warning("[%s:%d]TwoPointsPlanner Initialized failed", __FILE__, __LINE__);
         return false;
       }
       current_env_width_ = costmap_robot_->getCostmap()->getSizeInCellsX();
       current_env_height_ = costmap_robot_->getCostmap()->getSizeInCellsY();
       footprint_ = costmap_robot_->getRobotFootprint();
 
-      printf("TwoPointsPlanner Initialized successfully\n");
+      robot::log_info("TwoPointsPlanner Initialized successfully");
       initialized_ = true;
       return true;
     }
@@ -57,24 +57,25 @@ namespace two_points_planner
   {
     unsigned char result = 0;
 
-    // Bug
+
     if (!costmap_robot_)
     {
       std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl;
       return 0;
     }
 
-    // check if the costmap has an inflation layer
-    for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
-      layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
-      ++layer)
-    {
-      boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
-      if (!inflation_layer)
-        continue;
+    // Bug 
+    // // check if the costmap has an inflation layer
+    // for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
+    //   layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
+    //   ++layer)
+    // {
+    //   boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
+    //   if (!inflation_layer)
+    //     continue;
 
-      result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution()));
-    }
+    //   result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution()));
+    // }
     return result;
   }
 

src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt

@@ -0,0 +1,134 @@
+cmake_minimum_required(VERSION 3.10)
+project(pnkx_local_planner VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+# Build type
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+# Find system dependencies
+find_package(Boost REQUIRED COMPONENTS system)
+
+# Flags biên dịch
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# Thư mục include
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include
+)
+
+# Dependencies packages (internal libraries)
+set(PACKAGES_DIR
+    geometry_msgs
+    nav_msgs
+    std_msgs
+    sensor_msgs
+    visualization_msgs
+    nav_2d_utils
+    nav_core2
+    mkt_msgs
+    score_algorithm
+    costmap_2d
+    tf3
+    tf3_geometry_msgs
+    tf3_sensor_msgs
+    robot_cpp
+    angles
+)
+
+# Tạo thư viện utils trước
+add_library(${PROJECT_NAME}_utils SHARED
+    src/transforms.cpp
+)
+
+# Link libraries cho utils
+target_link_libraries(${PROJECT_NAME}_utils
+    PUBLIC ${PACKAGES_DIR}
+    PRIVATE Boost::boost
+)
+
+# Set include directories cho utils
+target_include_directories(${PROJECT_NAME}_utils
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# Tạo thư viện chính
+add_library(${PROJECT_NAME} SHARED
+    src/pnkx_local_planner.cpp
+    # src/pnkx_docking_local_planner.cpp
+    src/pnkx_go_straight_local_planner.cpp
+    src/pnkx_rotate_local_planner.cpp
+)
+
+# Link libraries cho thư viện chính
+target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${PROJECT_NAME}_utils
+    PUBLIC ${PACKAGES_DIR}
+    PRIVATE Boost::boost
+)
+
+# Set include directories cho thư viện chính
+target_include_directories(${PROJECT_NAME}
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# Compile options
+target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-terminate")
+
+# Tùy chọn build
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+#############
+## Install ##
+#############
+
+# Cài đặt header files
+install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include/${PROJECT_NAME}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+)
+
+# Cài đặt libraries
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
+    EXPORT ${PROJECT_NAME}-targets
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+# Export targets
+install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    DESTINATION lib/cmake/${PROJECT_NAME}
+)
+
+# Cài đặt plugins.xml nếu có
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml)
+    install(FILES
+        plugins.xml
+        DESTINATION share/${PROJECT_NAME}
+    )
+endif()
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
+message(STATUS "=================================")

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

@@ -0,0 +1,181 @@
+#ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
+#define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
+
+#include <nav_core/base_global_planner.h>
+#include <pnkx_local_planner/pnkx_local_planner.h>
+
+namespace pnkx_local_planner
+{
+
+  /**
+   * @class PNKXDockingLocalPlanner
+   * @brief Plugin-based flexible local_planner
+   */
+  class PNKXDockingLocalPlanner : public pnkx_local_planner::PNKXLocalPlanner
+  {
+  public:
+    /**
+     * @brief Constructor that brings up pluginlib loaders
+     */
+    PNKXDockingLocalPlanner();
+
+    virtual ~PNKXDockingLocalPlanner();
+
+    /**
+     * @brief nav_core2 initialization
+     * @param name Namespace for this planner
+     * @param tf TFListener pointer
+     * @param costmap_robot Costmap pointer
+     */
+    void initialize(robot::NodeHandle &parent, const std::string &name,
+                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+
+    /**
+     * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+     *
+     * It is presumed that the global plan is already set.
+     *
+     * This is mostly a wrapper for the protected computeVelocityCommands
+     * function which has additional debugging info.
+     *
+     * @param pose Current robot pose
+     * @param velocity Current robot velocity
+     * @return The best command for the robot to drive
+     */
+    virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
+                                                                const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+     *
+     * The pose that it checks against is the last pose in the current global plan.
+     * The calculation is delegated to the goal_checker plugin.
+     *
+     * @param pose Current pose
+     * @param velocity Current velocity
+     * @return True if the robot should be considered as having reached the goal.
+     */
+    virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Create a new instance of the PNKXDockingLocalPlanner
+     * @return A shared pointer to the new instance
+     */
+     static nav_core2::LocalPlanner::Ptr create();
+
+  protected:
+    /**
+     * @brief nav_core2 initialization other
+     */
+    void initializeOthers() override;
+    
+    /**
+     * @brief get docking maker
+     */
+    void getMaker();
+
+    /**
+     * @brief nav_core2 reset other
+     */
+    void reset() override;
+
+    /**
+     * @brief get parameters
+     * @param nh NodeHandle to read parameters from
+     */
+    void getParams(robot::NodeHandle &nh) override;
+
+    /**
+     * @brief Helper method for preparing for the core scoring algorithm
+     *
+     * Runs the prepare method on all the critics with freshly transformed data
+     */
+    virtual void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Iterate through all the twists and find the best one
+     */
+    virtual nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+  private:
+    class DockingPlanner
+    {
+    public:
+      DockingPlanner(const std::string &name);
+      ~DockingPlanner();
+
+      /**
+       * @brief initialization
+       * @param name Namespace for maker
+       * @param tf TFListener pointer
+       * @param costmap_robot Costmap pointer
+       */
+      void initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+                      score_algorithm::TrajectoryGenerator::Ptr traj_generator);
+
+      bool getMakerGoal(nav_2d_msgs::Pose2DStamped &maker_goal);
+
+      bool geLocalGoal(nav_2d_msgs::Pose2DStamped &local_goal);
+
+      bool getLocalPath(const nav_2d_msgs::Pose2DStamped &local_pose,
+                        const nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path);
+
+      bool initialized_;
+      bool is_detected_;
+      bool is_goal_reached_;
+      bool following_;
+      bool allow_rotate_;
+
+      double xy_goal_tolerance_;
+      double yaw_goal_tolerance_;
+      double min_lookahead_dist_;
+      double max_lookahead_dist_;
+      double lookahead_time_;
+      double angle_threshold_;
+      
+      std::string name_;
+      std::string docking_planner_name_;
+      std::string docking_nav_name_;
+
+      nav_core::BaseGlobalPlanner::Ptr docking_planner_;
+      score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
+
+      geometry_msgs::Vector3 linear_;
+      geometry_msgs::Vector3 angular_;
+
+    private:
+      /**
+       * @brief Callback wall timer for detect timeout
+       */
+      virtual void detectedTimeOutCb(const ::robot::WallTimerEvent &timer_event);
+
+      /**
+       * @brief Callback wall timer for delay
+       */
+      virtual void delayCb(const ::robot::WallTimerEvent &timer_event);
+
+      robot::NodeHandle nh_, nh_priv_;
+      TFListenerPtr tf_;
+      costmap_2d::Costmap2DROBOT *costmap_robot_;
+      score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
+      std::function<nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
+      std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
+
+      robot::WallTimer detected_timeout_wt_, delayed_wt_;
+      bool delayed_;
+      bool detected_timeout_;
+      std::string robot_base_frame_, maker_goal_frame_;
+    };
+
+    bool start_docking_;
+    double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
+    XmlRpc::XmlRpcValue original_papams_;
+    std::vector<DockingPlanner*> dkpl_;
+
+    bool dockingHanlde(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity);
+
+  }; // PNKXDockingLocalPlanner
+
+} // namespace pnkx_local_planner
+
+#endif // _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_

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

@@ -0,0 +1,86 @@
+#ifndef _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_
+#define _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_
+
+#include <pnkx_local_planner/pnkx_local_planner.h>
+
+namespace pnkx_local_planner
+{
+  /**
+   * @class PNKXGoStraightLocalPlanner
+   * @brief Plugin-based flexible local_planner
+   */
+  class PNKXGoStraightLocalPlanner : public pnkx_local_planner::PNKXLocalPlanner
+  {
+  public:
+    /**
+     * @brief Constructor that brings up pluginlib loaders
+     */
+    PNKXGoStraightLocalPlanner();
+
+    virtual ~PNKXGoStraightLocalPlanner();
+
+    /**
+     * @brief nav_core2 initialization
+     * @param name Namespace for this planner
+     * @param tf TFListener pointer
+     * @param costmap_robot Costmap pointer
+     */
+    void initialize(robot::NodeHandle &parent, const std::string &name,
+                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+
+    /**
+     * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+     *
+     * It is presumed that the global plan is already set.
+     *
+     * This is mostly a wrapper for the protected computeVelocityCommands
+     * function which has additional debugging info.
+     *
+     * @param pose Current robot pose
+     * @param velocity Current robot velocity
+     * @return The best command for the robot to drive
+     */
+    nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
+                                                        const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+     *
+     * The pose that it checks against is the last pose in the current global plan.
+     * The calculation is delegated to the goal_checker plugin.
+     *
+     * @param pose Current pose
+     * @param velocity Current velocity
+     * @return True if the robot should be considered as having reached the goal.
+     */
+    bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Create a new instance of the PNKXGoStraightLocalPlanner
+     * @return A shared pointer to the new instance
+     */
+    static nav_core2::LocalPlanner::Ptr create();
+
+  protected:
+    /**
+     * @brief nav_core2 reset other
+     */
+    virtual void reset() override;;
+
+    /**
+     * @brief Helper method for preparing for the core scoring algorithm
+     *
+     * Runs the prepare method on all the critics with freshly transformed data
+     */
+    void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Iterate through all the twists and find the best one
+     */
+    nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    bool is_ready_;
+  };
+
+} // namespace pnkx_local_planner
+#endif // _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_

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

@@ -0,0 +1,200 @@
+#ifndef _PNKX_LOCAL_PLANNER_H_INCLUDED_
+#define _PNKX_LOCAL_PLANNER_H_INCLUDED_
+
+#include <robot/node_handle.h>
+#include <robot/console.h>
+#include <nav_core2/local_planner.h>
+#include <score_algorithm/trajectory_generator.h>
+#include <score_algorithm/score_algorithm.h>
+#include <score_algorithm/goal_checker.h>
+#include <pnkx_local_planner/transforms.h>
+
+namespace pnkx_local_planner
+{
+
+  /**
+   * @class PNKXLocalPlanner
+   * @brief Plugin-based flexible local_planner
+   */
+  class PNKXLocalPlanner : public nav_core2::LocalPlanner
+  {
+  public:
+    /**
+     * @brief Constructor that brings up pluginlib loaders
+     */
+    PNKXLocalPlanner();
+
+    virtual ~PNKXLocalPlanner();
+
+    /**
+     * @brief nav_core2 initialization
+     * @param name Namespace for this planner
+     * @param tf TFListener pointer
+     * @param costmap_robot Costmap pointer
+     */
+    void initialize(robot::NodeHandle &parent, const std::string &name,
+                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+
+    /**
+     * @brief nav_core2 setGoalPose - Sets the global goal pose
+     * @param goal_pose The Goal Pose
+     */
+    void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) override;
+
+    /**
+     * @brief nav_core2 setPlan - Sets the global plan
+     * @param path The global plan
+     */
+    void setPlan(const nav_2d_msgs::Path2D &path) override;
+
+    /**
+     * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+     *
+     * It is presumed that the global plan is already set.
+     *
+     * This is mostly a wrapper for the protected computeVelocityCommands
+     * function which has additional debugging info.
+     *
+     * @param pose Current robot pose
+     * @param velocity Current robot velocity
+     * @return The best command for the robot to drive
+     */
+    nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
+                                                        const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief set velocity for x, y asix of the robot
+     * @param linear the velocity command
+     * @return True if set is done, false otherwise
+     */
+    bool setTwistLinear(geometry_msgs::Vector3 linear) override;
+
+    /**
+     * @brief get velocity for x, y asix of the robot
+     * @param linear The velocity command
+     * @return True if set is done, false otherwise
+     */
+    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
+
+    /**
+     * @brief Set velocity theta for z asix of the robot
+     * @param angular the command velocity
+     * @return True if set is done, false otherwise
+     */
+    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
+
+    /**
+     * @brief Get velocity theta for z asix of the robot
+     * @param direct The velocity command
+     * @return True if set is done, false otherwise
+     */
+    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
+
+    /**
+     * @brief Check if the kinematic parameters are currently locked
+     * @return True if the kinematic parameters are locked, false otherwise
+     *
+     * This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL.
+     */
+    virtual bool islock() override;
+
+    /**
+     * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+     *
+     * The pose that it checks against is the last pose in the current global plan.
+     * The calculation is delegated to the goal_checker plugin.
+     *
+     * @param pose Current pose
+     * @param velocity Current velocity
+     * @return True if the robot should be considered as having reached the goal.
+     */
+    bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Create a new PNKXLocalPlanner
+     * @return A shared pointer to the new PNKXLocalPlanner
+     */
+    static nav_core2::LocalPlanner::Ptr create();
+
+  protected:
+    /**
+     * @brief Lock the kinematic parameters to prevent modifications
+     *
+     * This function sets the kinematic parameters to a locked state, ensuring that values such as direction (xytheta_direct), velocities, or accelerations cannot be changed until unlocked. Locking during critical operations like in-place rotation may stabilize motion commands but could cause issues if the lock persists too long, preventing necessary updates to odometry data used by AMCL.
+     */
+    virtual void lock();
+
+    /**
+     * @brief Unlock the kinematic parameters to allow modifications
+     *
+     * This function releases the lock on kinematic parameters, allowing changes to values such as direction, velocities, or accelerations. If the parameters are unlocked prematurely during in-place rotation, sudden changes to motion parameters (e.g., theta direction) could lead to odometry discontinuities, contributing to localization jumps in AMCL.
+     */
+    virtual void unlock();
+    /**
+     * @brief nav_core2 initialization other
+     */
+    virtual void initializeOthers();
+
+    /**
+     * @brief nav_core2 reset other
+     */
+    virtual void reset();
+
+    /**
+     * @brief Get parameters
+     * @param nh NodeHandle to read parameters from
+     */
+    virtual void getParams(robot::NodeHandle &nh);
+
+    /**
+     * @brief Helper method for preparing for the core scoring algorithm
+     *
+     * Runs the prepare method on all the critics with freshly transformed data
+     */
+    virtual void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity);
+
+    /**
+     * @brief Iterate through all the twists and find the best one
+     */
+    virtual nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity);
+
+    /**
+     * @brief Helper method to transform a given pose to the local costmap frame.
+     */
+    nav_2d_msgs::Pose2DStamped transformPoseToLocal(const nav_2d_msgs::Pose2DStamped &pose);
+
+    // Plugin handling
+    std::function<score_algorithm::TrajectoryGenerator::Ptr()> traj_gen_loader_;
+    score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
+
+    std::function<score_algorithm::ScoreAlgorithm::Ptr()> nav_algorithm_loader_;
+    score_algorithm::ScoreAlgorithm::Ptr nav_algorithm_;
+    std::function<score_algorithm::ScoreAlgorithm::Ptr()> rotate_algorithm_loader_;
+    score_algorithm::ScoreAlgorithm::Ptr rotate_algorithm_;
+
+    std::function<score_algorithm::GoalChecker::Ptr()> goal_checker_loader_;
+    score_algorithm::GoalChecker::Ptr goal_checker_;
+
+    bool initialized_;
+    TFListenerPtr tf_;
+    std::string name_;
+    robot::NodeHandle parent_, planner_nh_;  
+    nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan
+    nav_2d_msgs::Path2D transformed_plan_;
+    nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
+
+    costmap_2d::Costmap2D *costmap_;
+    costmap_2d::Costmap2DROBOT *costmap_robot_;
+    nav_grid::NavGridInfo info_;
+    boost::recursive_mutex configuration_mutex_;
+    bool update_costmap_before_planning_;
+
+    bool ret_angle_, ret_nav_;
+    double yaw_goal_tolerance_, xy_goal_tolerance_;
+    bool lock_;
+
+  }; // PNKXLocalPlanner
+
+} // namespace pnkx_local_planner
+
+#endif // _PNKX_LOCAL_PLANNER_H_INCLUDED_

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

@@ -0,0 +1,85 @@
+#ifndef _PNKX_ROTATE_LOCAL_PLANNER_H_INCLUDED_
+#define _PNKX_ROTATE_LOCAL_PLANNER_H_INCLUDED_
+
+#include <pnkx_local_planner/pnkx_local_planner.h>
+
+namespace pnkx_local_planner
+{
+  /**
+   * @class PNKXRotateLocalPlanner
+   * @brief Plugin-based flexible local_planner
+   */
+  class PNKXRotateLocalPlanner : public pnkx_local_planner::PNKXLocalPlanner
+  {
+  public:
+    /**
+     * @brief Constructor that brings up pluginlib loaders
+     */
+    PNKXRotateLocalPlanner();
+
+    virtual ~PNKXRotateLocalPlanner();
+
+    /**
+     * @brief nav_core2 initialization
+     * @param name Namespace for this planner
+     * @param tf TFListener pointer
+     * @param costmap_robot Costmap pointer
+     */
+    void initialize(robot::NodeHandle &parent, const std::string &name,
+                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+
+    /**
+     * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+     *
+     * It is presumed that the global plan is already set.
+     *
+     * This is mostly a wrapper for the protected computeVelocityCommands
+     * function which has additional debugging info.
+     *
+     * @param pose Current robot pose
+     * @param velocity Current robot velocity
+     * @return The best command for the robot to drive
+     */
+    nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
+                                                        const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+     *
+     * The pose that it checks against is the last pose in the current global plan.
+     * The calculation is delegated to the goal_checker plugin.
+     *
+     * @param pose Current pose
+     * @param velocity Current velocity
+     * @return True if the robot should be considered as having reached the goal.
+     */
+    bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Create a new instance of the PNKXRotateLocalPlanner
+     * @return A shared pointer to the new instance
+     */
+    static nav_core2::LocalPlanner::Ptr create();
+
+  protected:
+    /**
+     * @brief nav_core2 reset other
+     */
+    virtual void reset() override;
+
+    /**
+     * @brief Helper method for preparing for the core scoring algorithm
+     *
+     * Runs the prepare method on all the critics with freshly transformed data
+     */
+    void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+    /**
+     * @brief Iterate through all the twists and find the best one
+     */
+    nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
+
+  };
+}
+
+#endif // _PNKX_ROTATE_LOCAL_PLANNER_H_INCLUDED_

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

@@ -0,0 +1,50 @@
+#ifndef _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__
+#define _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__
+
+#include <robot/node_handle.h>
+#include <nav_2d_msgs/Pose2DStamped.h>
+#include <nav_2d_utils/conversions.h>
+
+#include <nav_core2/common.h>
+#include <nav_core2/exceptions.h>
+
+#include <tf3/buffer_core.h>
+#include <tf3/convert.h>
+#include <tf3/utils.h>
+#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+
+#include <costmap_2d/costmap_2d_robot.h>
+
+namespace pnkx_local_planner
+{
+  bool getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame,
+               nav_2d_msgs::Pose2DStamped &global_pose, double transform_tolerance = 2.0);
+
+  bool transformGlobalPose(TFListenerPtr tf, const std::string &global_frame,
+                           const nav_2d_msgs::Pose2DStamped &stamped_pose, nav_2d_msgs::Pose2DStamped &newer_pose);
+
+  double getSquareDistance(const geometry_msgs::Pose2D &pose_a, const geometry_msgs::Pose2D &pose_b);
+
+  /**
+   * @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 nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose,
+      const costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length,
+      nav_2d_msgs::Path2D &transformed_plan, const bool from_global_frame = false);
+
+}
+
+#endif

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

@@ -0,0 +1,772 @@
+#include <pnkx_local_planner/pnkx_docking_local_planner.h>
+#include <nav_core2/exceptions.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+#include <nav_2d_utils/parameters.h>
+#include <nav_2d_utils/footprint.h>
+#include <angles/angles.h>
+#include <nav_msgs/Path.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/UInt16.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <boost/dll/import.hpp>
+#include <boost/dll/alias.hpp>
+
+pnkx_local_planner::PNKXDockingLocalPlanner::PNKXDockingLocalPlanner()
+    : start_docking_(false)
+{
+}
+
+pnkx_local_planner::PNKXDockingLocalPlanner::~PNKXDockingLocalPlanner()
+{
+  if (traj_generator_)
+    traj_generator_.reset();
+
+  if (nav_algorithm_)
+    nav_algorithm_.reset();
+
+  if (rotate_algorithm_)
+    rotate_algorithm_.reset();
+
+  if (goal_checker_)
+    goal_checker_.reset();
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
+                                                      TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot)
+{
+  if (!initialized_)
+  {
+    tf_ = tf;
+    name_ = name;
+    costmap_robot_ = costmap_robot;
+    costmap_ = costmap_robot_->getCostmap();
+
+    info_.width = costmap_->getSizeInCellsX();
+    info_.height = costmap_->getSizeInCellsY();
+    info_.resolution = costmap_->getResolution();
+    info_.frame_id = costmap_robot_->getGlobalFrameID();
+    info_.origin_x = costmap_->getOriginX();
+    info_.origin_y = costmap_->getOriginY();
+
+    if (!costmap_robot_->isCurrent())
+      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+
+    nh_ = robot::NodeHandle("~");
+    parent_ = parent;
+    planner_nh_ = robot::NodeHandle(parent_, name);
+    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s", planner_nh_.getNamespace().c_str());
+
+    this->getParams(planner_nh_);
+
+    std::string traj_generator_name;
+    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
+    robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
+        path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
+      traj_generator_ = traj_gen_loader_();
+      if (!traj_generator_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_traj_gen = robot::NodeHandle(nh_, traj_generator_name);
+      traj_generator_->initialize(nh_traj_gen);
+      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str());
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
+      exit(1);
+    }
+
+    std::string algorithm_nav_name;
+    planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+    robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+        path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
+      nav_algorithm_ = algorithm_loader_();
+      if (!nav_algorithm_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
+        exit(1);
+      }
+      nav_algorithm_->initialize(nh_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
+    }
+    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);
+    }
+
+    std::string algorithm_rotate_name;
+    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff"));
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+        path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
+      rotate_algorithm_ = algorithm_loader_();
+      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);
+      }
+      rotate_algorithm_->initialize(nh_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
+      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
+      exit(1);
+    }
+
+    std::string goal_checker_name;
+    planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
+        path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
+      goal_checker_ = goal_checker_loader_();
+      if (!goal_checker_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
+        exit(1);
+      }
+      goal_checker_->initialize(nh_);
+      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
+      exit(1);
+    }
+    this->initializeOthers();
+    this->getMaker();
+    robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
+    initialized_ = true;
+  }
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
+{
+  std::string maker_name, maker_sources;
+  nh_.param("maker_name", maker_name, std::string(""));
+  nh_.param("maker_sources", maker_sources, std::string(""));
+  std::stringstream ss(maker_sources);
+  std::string source;
+
+  while (ss >> source)
+  {
+    if (maker_name == source)
+    {
+      robot::NodeHandle source_node(nh_, source);
+
+      if (source_node.hasParam("plugins"))
+      {
+        XmlRpc::XmlRpcValue plugins;
+        source_node.getParam("plugins", plugins);
+
+        if (plugins.getType() == XmlRpc::XmlRpcValue::TypeArray)
+        {
+          for (int i = 0; i < plugins.size(); ++i)
+          {
+            if (plugins[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
+            {
+              std::stringstream name;
+              name << source << "/" << static_cast<std::string>(plugins[i]["name"]);
+              std::string docking_planner_name = static_cast<std::string>(plugins[i]["docking_planner"]);
+              std::string docking_nav_name = static_cast<std::string>(plugins[i]["docking_nav"]);
+
+              // std::shared_ptr<DockingPlanner> dkpl = std::make_shared<DockingPlanner>(name.str());
+              DockingPlanner* dkpl = new DockingPlanner(name.str());
+              dkpl->docking_planner_name_ = docking_planner_name;
+              dkpl->docking_nav_name_ = docking_nav_name;
+              dkpl_.push_back(dkpl);
+            }
+          }
+        }
+        else
+        {
+          robot::log_warning_at(__FILE__, __LINE__, "Expected 'plugins' to be an array under namespace [%s]", source.c_str());
+        }
+      }
+      else
+      {
+        robot::log_warning_at(__FILE__, __LINE__, "No 'plugins' found under namespace [%s]", source.c_str());
+      }
+      break;
+    }
+  }
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::initializeOthers()
+{
+  std::string algorithm_nav_name;
+  planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+  if (!nh_.getParam(algorithm_nav_name, original_papams_))
+    robot::log_warning_at(__FILE__, __LINE__, "No found in %s in yaml-file, please check configuration", algorithm_nav_name.c_str());
+  original_xy_goal_tolerance_ = xy_goal_tolerance_;
+  original_yaw_goal_tolerance_ = yaw_goal_tolerance_;
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::getParams(robot::NodeHandle &nh)
+{
+  // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window
+  nh.param("update_costmap_before_planning", update_costmap_before_planning_, true);
+  if (!nh.param<double>("xy_goal_tolerance", xy_goal_tolerance_, 0.1))
+  {
+    xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1);
+  }
+  if (!nh.param<double>("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1))
+  {
+    yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
+  }
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
+{
+  robot::log_info_at(__FILE__, __LINE__, "New Docking Goal Received.");
+  this->unlock();
+  if(traj_generator_) traj_generator_->reset();
+  if(goal_checker_) goal_checker_->reset();
+  if(nav_algorithm_) nav_algorithm_->reset();
+  if(rotate_algorithm_) rotate_algorithm_->reset();
+  ret_nav_ = ret_angle_ = false;
+
+  std::string algorithm_nav_name;
+  planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+  robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
+  nh_.setParam(algorithm_nav_name, original_papams_);
+  nh_algorithm.setParam("allow_rotate", false);
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{  
+  this->getParams(planner_nh_);
+  if (update_costmap_before_planning_)
+  {
+    info_.width = costmap_->getSizeInCellsX();
+    info_.height = costmap_->getSizeInCellsY();
+    info_.resolution = costmap_->getResolution();
+    info_.frame_id = costmap_robot_->getGlobalFrameID();
+    info_.origin_x = costmap_->getOriginX();
+    info_.origin_y = costmap_->getOriginY();
+
+    if (!costmap_robot_->isCurrent())
+      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+  }
+
+  // Update time stamp of goal pose
+  // goal_pose_.header.stamp = pose.header.stamp;
+  nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
+                             local_goal_pose = this->transformPoseToLocal(goal_pose_);
+  if (start_docking_)
+  {
+    local_goal_pose = goal_pose_;
+  }
+
+  try
+  {
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+      robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
+  }
+  catch(const nav_core2::LocalPlannerException& e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+  }
+
+  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))
+    {
+      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
+      throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+    }
+    // else
+    //   ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction);
+
+    if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
+    {
+      this->lock();
+      geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
+      traj_generator_->setTwistLinear(linear);
+      linear.x *= (-1);
+      traj_generator_->setTwistLinear(linear);
+      traj_generator_->setTwistAngular(dkpl_.front()->angular_);
+
+      std::string algorithm_nav_name;
+      planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+      robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
+      nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_);
+      nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_);
+      nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_);
+      nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
+      nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
+
+      nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+
+      if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
+      {
+
+        if (dkpl_.front()->following_)
+        {
+          nav_2d_msgs::Pose2DStamped follow_pose;
+          if (dkpl_.front()->geLocalGoal(local_goal_pose))
+          {
+            local_goal_pose = follow_pose;
+          }
+        }
+        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+        {
+          throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+          robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
+        }
+      }
+    }
+  }
+  else
+  {
+    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    {
+      throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
+    }
+    // else
+    //   ROS_INFO("chieu %f %f %f", x_direction, y_direction, theta_direction);
+  }
+
+  int xytheta_direct[3];
+  xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0;
+  xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0;
+  xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0;
+  traj_generator_->setDirect(xytheta_direct);
+}
+
+nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
+                                                                                                 const nav_2d_msgs::Twist2D &velocity)
+{
+  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  nav_2d_msgs::Twist2DStamped cmd_vel;
+  try
+  {
+    if (global_plan_.poses.empty())
+      return cmd_vel;
+    this->prepare(pose, velocity);
+    cmd_vel = this->ScoreAlgorithm(pose, velocity);
+    return cmd_vel;
+  }
+  catch (const nav_core2::PlannerException &e)
+  {
+    robot::log_warning_at(__FILE__, __LINE__, "%s", e.what());
+    throw nav_core2::LocalPlannerException(e.what());
+  }
+}
+
+nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{
+  nav_2d_msgs::Twist2D twist;
+  mkt_msgs::Trajectory2D traj;
+  nav_2d_msgs::Twist2DStamped cmd_vel;
+  cmd_vel.header.stamp = robot::Time::now();
+
+  if (ret_nav_ && ret_angle_)
+    return cmd_vel;
+  else if (!ret_nav_)
+  {
+    traj = nav_algorithm_->calculator(pose, velocity);
+
+    if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
+    {
+      if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
+      {
+        traj = dkpl_.front()->docking_nav_->calculator(pose, velocity);
+      }
+    }
+  }
+  else
+    traj = rotate_algorithm_->calculator(pose, velocity);
+
+  cmd_vel.velocity = traj.velocity;
+  return cmd_vel;
+}
+
+bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{
+  if (goal_pose_.header.frame_id == "")
+  {
+    robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
+    return false;
+  }
+
+  bool dock_ok = dockingHanlde(pose, velocity);
+
+  // Update time stamp of goal pose
+  // goal_pose_.header.stamp = pose.header.stamp;
+  nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
+  nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
+  nav_2d_msgs::Path2D plan = transformed_plan_;
+  if (start_docking_)
+  {
+    local_goal = goal_pose_;
+    if (!dkpl_.empty() && dkpl_.front()->initialized_)
+    {
+      if (dkpl_.front()->allow_rotate_)
+        plan = nav_2d_msgs::Path2D();
+    }
+  }
+
+  if (!ret_nav_)
+  {
+    ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity);
+
+    if (ret_nav_)
+    {
+      robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta);
+      robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta);
+      robot::log_info_at(__FILE__, __LINE__, "goal_pose_ %f %f %f", goal_pose_.pose.x, goal_pose_.pose.y, goal_pose_.pose.theta);
+    }
+  }
+
+  if (ret_nav_ && !ret_angle_ && !dock_ok)
+  {
+    double delta_orient =
+        fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta));
+    ret_angle_ = (bool)(delta_orient <= yaw_goal_tolerance_);
+    if(ret_angle_)
+      robot::log_warning_at(__FILE__, __LINE__, "xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_);
+  }
+
+  if (start_docking_)
+  {
+    double dx = local_goal.pose.x - local_pose.pose.x;
+    double dy = local_goal.pose.y - local_pose.pose.y;
+    double dtheta = local_goal.pose.theta - local_pose.pose.theta;
+  }
+  if (ret_nav_ && ret_angle_ && dock_ok)
+  {
+    // nh_.setParam("xy_goal_tolerance", original_xy_goal_tolerance_);
+    // nh_.setParam("yaw_goal_tolerance", original_yaw_goal_tolerance_);
+    std::string algorithm_nav_name;
+    planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+    robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
+    nh_.setParam(algorithm_nav_name, original_papams_);
+    nh_algorithm.setParam("allow_rotate", false);
+  }
+  return ret_nav_ && ret_angle_ && dock_ok;
+}
+
+bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{
+  if (!dkpl_.empty())
+  {
+    if (ret_nav_ && ret_angle_)
+    {
+      if (!dkpl_.front()->is_detected_)
+      {
+        if (!dkpl_.front()->initialized_)
+        {
+          dkpl_.front()->initialize(nh_, tf_, costmap_robot_, traj_generator_);
+          dkpl_.front()->initialized_ = true;
+        }
+        else
+        {
+          if (dkpl_.front()->docking_planner_ && !dkpl_.front()->docking_nav_)
+          {
+            nav_2d_msgs::Pose2DStamped local_goal;
+            try
+            {
+              if (dkpl_.front()->geLocalGoal(local_goal))
+              {
+                dkpl_.front()->is_detected_ = true;
+                start_docking_ = true;
+                nav_msgs::Path path;
+                nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
+                dkpl_.front()->getLocalPath(local_pose, local_goal, path);
+                this->setPlan(nav_2d_utils::pathToPath(path));
+                this->setGoalPose(local_goal);
+              }
+            }
+            catch (const std::exception &e)
+            {
+              throw nav_core2::LocalPlannerException(e.what());
+            }
+          }
+          else if (!dkpl_.front()->docking_planner_ && dkpl_.front()->docking_nav_)
+          {
+            nav_2d_msgs::Pose2DStamped local_goal;
+            try
+            {
+              if (dkpl_.front()->geLocalGoal(local_goal))
+              {
+                dkpl_.front()->is_detected_ = true;
+                start_docking_ = true;
+                nav_2d_msgs::Path2D path;
+                nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
+                path.header.stamp = robot::Time::now();
+                path.header.frame_id = costmap_robot_->getGlobalFrameID();
+                path.poses.push_back(local_pose);
+                path.poses.push_back(local_goal);
+                this->setPlan(path);
+                this->setGoalPose(local_goal);
+              }
+            }
+            catch (const std::exception &e)
+            {
+              throw nav_core2::LocalPlannerException(e.what());
+            }
+          }
+        }
+      }
+      else
+      {
+        if (dkpl_.front()->initialized_ && !dkpl_.front()->is_goal_reached_)
+        {
+          dkpl_.front()->is_goal_reached_ = true;
+          robot::log_info_at(__FILE__, __LINE__, "%s is goal reached", dkpl_.front()->name_.c_str());
+        }
+
+        if (dkpl_.front()->initialized_ && dkpl_.front()->is_goal_reached_)
+        {
+          if(!dkpl_.empty()) 
+          {
+            if(dkpl_.front()) delete(dkpl_.front());
+            dkpl_.erase(dkpl_.begin());
+          }
+          start_docking_ = false;
+        }
+      }
+    }
+  }
+  bool result = false;
+  if (dkpl_.empty())
+  {
+    robot::log_info_at(__FILE__, __LINE__, "Goal reached!");
+    result = true;
+    this->getMaker();
+  }
+  return result;
+}
+
+pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name)
+    : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false),
+      is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
+{
+}
+
+pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
+{
+  if (docking_planner_)
+    docking_planner_.reset();
+  if (docking_nav_)
+    docking_nav_.reset();
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+                                                                             score_algorithm::TrajectoryGenerator::Ptr traj_generator)
+{
+  nh_ = nh;
+  nh_priv_ = robot::NodeHandle(nh_, name_);
+  tf_ = tf;
+  costmap_robot_ = costmap_robot;
+  traj_generator_ = traj_generator;
+
+  if (docking_planner_name_ != std::string(""))
+  {
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      docking_planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
+        path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
+      docking_planner_ = docking_planner_loader_();
+      if (!docking_planner_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create global planner \"%s\": returned null pointer", docking_planner_name_.c_str());
+        exit(1);
+      }
+      docking_planner_->initialize(name_, costmap_robot_);
+      robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str());
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s",
+                docking_planner_name_.c_str(), ex.what());
+    }
+  }
+
+  if (docking_nav_name_ != std::string(""))
+  {
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      docking_nav_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+        path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations);
+      docking_nav_ = docking_nav_loader_();
+      if (!docking_nav_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_, docking_nav_name_);
+      docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);
+      robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str());
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s",
+                docking_nav_name_.c_str(), ex.what());
+    }
+  }
+
+  robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
+  if (!nh_priv_.param("maker_goal_frame", maker_goal_frame_, std::string("")))
+  {
+    std::stringstream re;
+    re << "Can not get 'maker_goal_frame' in " << name_;
+    robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
+    throw nav_core2::LocalPlannerException(re.str());
+  }
+
+  nh_priv_.param("vel_x", linear_.x, 0.5);
+  nh_priv_.param("vel_theta", angular_.z, 0.5);
+  nh_priv_.param("following", following_, false);
+  nh_priv_.param("allow_rotate", allow_rotate_, false);
+
+  double delay_time, time_out;
+  nh_priv_.param("delay", delay_time, 1.0);
+  nh_priv_.param("timeout", time_out, 1.0);
+
+  nh_priv_.param("xy_goal_tolerance", xy_goal_tolerance_, 0.05);
+  nh_priv_.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
+
+  nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.4);
+  nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 1.0);
+  nh_priv_.param("lookahead_time", lookahead_time_, 1.5);
+  nh_priv_.param("angle_threshold", angle_threshold_, 0.4);
+
+  detected_timeout_wt_ =
+      nh_priv_.createWallTimer(::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this);
+  detected_timeout_wt_.start();
+  robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
+
+  delayed_wt_ =
+      nh_priv_.createWallTimer(::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this);
+  delayed_wt_.start();
+  robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
+
+  initialized_ = true;
+}
+
+bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getMakerGoal(nav_2d_msgs::Pose2DStamped &maker_goal)
+{
+  if (!delayed_)
+    return false;
+
+  bool get_pose_result = false;
+  try
+  {
+    if (pnkx_local_planner::getPose(tf_, maker_goal_frame_, robot_base_frame_, maker_goal, 2.0))
+    {
+      get_pose_result = true;
+      detected_timeout_wt_.stop();
+    }
+
+    if (get_pose_result)
+      return true;
+  }
+  catch (const std::exception &e)
+  {
+    if (detected_timeout_)
+    {
+      std::stringstream re;
+      re << "No detected " << maker_goal_frame_;
+      robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
+      throw nav_core2::LocalPlannerException(re.str());
+    }
+  }
+  return false;
+}
+
+bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(nav_2d_msgs::Pose2DStamped &local_goal)
+{
+  if (!delayed_)
+    return false;
+  bool get_pose_result = false;
+  try
+  {
+    nav_2d_msgs::Pose2DStamped maker_goal_on_robot;
+    if (pnkx_local_planner::getPose(tf_, maker_goal_frame_, robot_base_frame_, maker_goal_on_robot, 2.0))
+    {
+      if (nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), maker_goal_on_robot, local_goal))
+      {
+        get_pose_result = true;
+        detected_timeout_wt_.stop();
+      }
+    }
+
+    if (get_pose_result)
+      return true;
+  }
+  catch (const std::exception &e)
+  {
+    if (detected_timeout_)
+    {
+      std::stringstream re;
+      re << "No detected " << maker_goal_frame_;
+      robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
+      throw nav_core2::LocalPlannerException(re.str());
+    }
+  }
+  return false;
+}
+
+bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
+    const nav_2d_msgs::Pose2DStamped &local_pose, const nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path)
+{
+  geometry_msgs::PoseStamped start = nav_2d_utils::pose2DToPoseStamped(local_pose);
+  geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(local_goal);
+  std::vector<geometry_msgs::PoseStamped> docking_plan;
+
+  if (!docking_planner_->makePlan(start, goal, docking_plan))
+  {
+    throw nav_core2::LocalPlannerException("Making plan from goal maker is failed");
+    return false;
+  }
+  else
+  {
+    local_path = nav_2d_utils::posesToPath(docking_plan);
+    return true;
+  }
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb(const ::robot::WallTimerEvent &timer_event)
+{
+  detected_timeout_ = true;
+  detected_timeout_wt_.stop();
+  robot::log_warning_at(__FILE__, __LINE__, "%s time_out end %f", name_.c_str(), robot::Time::now().toSec());
+}
+
+void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb(const ::robot::WallTimerEvent &timer_event)
+{
+  delayed_ = true;
+  delayed_wt_.stop();
+  robot::log_warning_at(__FILE__, __LINE__, "%s delay end %f", name_.c_str(), robot::Time::now().toSec());
+}
+
+nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXDockingLocalPlanner::create()
+{
+  return std::make_shared<PNKXDockingLocalPlanner>();
+}
+
+//  register this planner as a LocalPlanner plugin
+BOOST_DLL_ALIAS(pnkx_local_planner::PNKXDockingLocalPlanner::create, PNKXDockingLocalPlanner)

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

@@ -0,0 +1,238 @@
+#include <nav_core2/exceptions.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+#include <nav_2d_utils/parameters.h>
+#include <mkt_msgs/Trajectory2D.h>
+
+// #include <g2o/core/base_binary_edge.h>
+// #include <g2o/core/base_unary_edge.h>
+// #include <g2o/core/base_multi_edge.h>
+#include <angles/angles.h>
+#include <nav_msgs/Path.h>
+#include "pnkx_local_planner/pnkx_go_straight_local_planner.h"
+#include <boost/dll/import.hpp>
+#include <boost/dll/alias.hpp>
+
+pnkx_local_planner::PNKXGoStraightLocalPlanner::PNKXGoStraightLocalPlanner()
+{
+}
+
+pnkx_local_planner::PNKXGoStraightLocalPlanner::~PNKXGoStraightLocalPlanner()
+{
+  if (traj_generator_)
+    traj_generator_.reset();
+
+  if (nav_algorithm_)
+    nav_algorithm_.reset();
+
+  if (goal_checker_)
+    goal_checker_.reset();
+}
+
+void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot)
+{
+  if (!initialized_)
+  {
+    robot::log_info_at(__FILE__, __LINE__, "Using parent name \"%s\"", name.c_str());
+    tf_ = tf;
+    costmap_robot_ = costmap_robot;
+    costmap_ = costmap_robot_->getCostmap();
+
+    info_.width = costmap_->getSizeInCellsX();
+    info_.height = costmap_->getSizeInCellsY();
+    info_.resolution = costmap_->getResolution();
+    info_.frame_id = costmap_robot_->getGlobalFrameID();
+    info_.origin_x = costmap_->getOriginX();
+    info_.origin_y = costmap_->getOriginY();
+
+    if (!costmap_robot_->isCurrent())
+      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+    
+    parent_ = parent;
+    planner_nh_ = robot::NodeHandle(parent_, name);
+    
+    this->getParams(planner_nh_);
+ 
+    // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window
+    std::string traj_generator_name;
+    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
+
+    robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
+        path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
+      traj_generator_ = traj_gen_loader_();
+      if (!traj_generator_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
+      traj_generator_->initialize(nh_traj_gen);
+    }
+    catch (const std::exception& ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
+      exit(1);
+    }
+
+    std::string algorithm_nav_name = std::string("pnkx_local_planner::GoStraight");
+    planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::GoStraight"));
+    robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+        path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
+      nav_algorithm_ = nav_algorithm_loader_();
+      if (!nav_algorithm_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+      nav_algorithm_->initialize(nh_algorithm, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
+    }
+    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);
+    }
+
+    std::string goal_checker_name;
+    planner_nh_.param("goal_checker_name", goal_checker_name, std::string("score_algorithm::GoalChecker"));
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
+        path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
+      goal_checker_ = goal_checker_loader_();
+      if (!goal_checker_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
+      goal_checker_->initialize(nh_goal_checker);
+    }
+    catch (const std::exception& ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
+      exit(1);
+    }
+
+    robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str());
+    initialized_ = true;
+  }
+}
+
+void pnkx_local_planner::PNKXGoStraightLocalPlanner::reset()
+{
+  robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
+  this->unlock();
+  traj_generator_->reset();
+  goal_checker_->reset();
+  nav_algorithm_->reset();
+  ret_nav_ = ret_angle_ = false;
+}
+
+nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
+  const nav_2d_msgs::Twist2D& velocity)
+{
+  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  nav_2d_msgs::Twist2DStamped cmd_vel;
+  try
+  {
+    if (global_plan_.poses.empty())
+      return cmd_vel;
+    this->prepare(pose, velocity);
+    cmd_vel = this->ScoreAlgorithm(pose, velocity);
+    return cmd_vel;
+  }
+  catch (const nav_core2::PlannerException &e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+  }
+}
+
+void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity)
+{
+  this->getParams(planner_nh_);
+  if (update_costmap_before_planning_)
+  {
+    info_.width = costmap_->getSizeInCellsX();
+    info_.height = costmap_->getSizeInCellsY();
+    info_.resolution = costmap_->getResolution();
+    info_.frame_id = costmap_robot_->getGlobalFrameID();
+    info_.origin_x = costmap_->getOriginX();
+    info_.origin_y = costmap_->getOriginY();
+
+    if (!costmap_robot_->isCurrent())
+      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+  }
+  // Update time stamp of goal pose
+  // goal_pose_.header.stamp = pose.header.stamp;
+  nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
+    local_goal_pose = this->transformPoseToLocal(goal_pose_);
+
+  try
+  {
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+      throw nav_core2::LocalPlannerException("Transform global plan is failed");
+  }
+  catch(const nav_core2::LocalPlannerException& e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+  }
+
+  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))
+  {
+    throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+  }
+
+  int xytheta_direct[3];
+  xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0;
+  xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0;
+  xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0;
+  traj_generator_->setDirect(xytheta_direct);
+}
+
+nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity)
+{
+  nav_2d_msgs::Twist2D twist;
+  mkt_msgs::Trajectory2D traj;
+  nav_2d_msgs::Twist2DStamped cmd_vel;
+  if (!ret_nav_)
+    traj = nav_algorithm_->calculator(pose, velocity);
+
+  cmd_vel.velocity = traj.velocity;
+  return cmd_vel;
+}
+
+bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity)
+{
+  if (goal_pose_.header.frame_id == "")
+  {
+    robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
+    return false;
+  }
+  // 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);
+
+  if (ret_nav_)
+    robot::log_info_at(__FILE__, __LINE__, "Goal reached!");
+  return ret_nav_;
+}
+
+nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXGoStraightLocalPlanner::create()
+{
+  return std::make_shared<pnkx_local_planner::PNKXGoStraightLocalPlanner>();
+}
+
+//  register this planner as a LocalPlanner plugin
+BOOST_DLL_ALIAS(pnkx_local_planner::PNKXGoStraightLocalPlanner::create, PNKXGoStraightLocalPlanner)

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

@@ -0,0 +1,444 @@
+#include <pnkx_local_planner/pnkx_local_planner.h>
+#include <nav_core2/exceptions.h>
+
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+#include <nav_2d_utils/parameters.h>
+
+// #include <g2o/core/base_binary_edge.h>
+// #include <g2o/core/base_unary_edge.h>
+// #include <g2o/core/base_multi_edge.h>
+#include <angles/angles.h>
+
+#include <tf3/convert.h>
+#include <tf3/utils.h>
+#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+
+#include <mkt_msgs/Trajectory2D.h>
+#include <boost/dll/import.hpp>
+#include <boost/dll/alias.hpp>
+
+pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner()
+  : initialized_(false)
+{
+}
+
+pnkx_local_planner::PNKXLocalPlanner::~PNKXLocalPlanner()
+{
+  if (traj_generator_)
+    traj_generator_.reset();
+
+  if (nav_algorithm_)
+    nav_algorithm_.reset();
+
+  if (rotate_algorithm_)
+    rotate_algorithm_.reset();
+
+  if (goal_checker_)
+    goal_checker_.reset();
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
+                                                      TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot)
+{
+  if (!initialized_)
+  {
+    tf_ = tf;
+    name_ = name;
+    costmap_robot_ = costmap_robot;
+    costmap_ = costmap_robot_->getCostmap();
+
+    info_.width = costmap_->getSizeInCellsX();
+    info_.height = costmap_->getSizeInCellsY();
+    info_.resolution = costmap_->getResolution();
+    info_.frame_id = costmap_robot_->getGlobalFrameID();
+    info_.origin_x = costmap_->getOriginX();
+    info_.origin_y = costmap_->getOriginY();
+
+    if (!costmap_robot_->isCurrent())
+      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+
+    parent_ = parent;
+    robot::log_warning_at(__FILE__, __LINE__, "Parent namespace: %s", parent_.getNamespace().c_str());
+    planner_nh_ = robot::NodeHandle(parent_, name);
+    // planner_nh_.printRootParams();
+    this->getParams(planner_nh_);
+
+    std::string traj_generator_name;
+    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("StandardTrajectoryGenerator"));
+    robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_plugins/libmkt_plugins_standard_traj_generator.so";
+      traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
+        path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
+      traj_generator_ = traj_gen_loader_();
+      if (!traj_generator_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
+      traj_generator_->initialize(nh_traj_gen);
+      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str());
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
+      exit(1);
+    }
+    
+    std::string algorithm_nav_name; 
+    planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+    robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_algorithm/libmkt_algorithm_diff.so";
+      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+        path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
+      nav_algorithm_ = nav_algorithm_loader_();
+      if (!nav_algorithm_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
+        exit(1);
+      }
+      nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
+    }
+    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);
+    }
+
+    std::string algorithm_rotate_name;
+    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal"));
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_algorithm/libmkt_algorithm_diff.so";
+      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+        path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
+      rotate_algorithm_ = rotate_algorithm_loader_();
+      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);
+      }
+      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());
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
+      exit(1);
+    }
+
+    std::string goal_checker_name;
+    planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_plugins/libmkt_plugins_goal_checker.so";
+      goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
+        path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
+      goal_checker_ = goal_checker_loader_();
+      if (!goal_checker_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
+        exit(1);
+      }
+      goal_checker_->initialize(parent_);
+      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
+      exit(1);
+    }
+    this->initializeOthers();
+    robot::log_info("[%s:%d]\n %s is sucessed", __FILE__, __LINE__, name.c_str());
+    initialized_ = true;
+  }
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::initializeOthers()
+{
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::getParams(robot::NodeHandle &nh)
+{
+  // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window
+  nh.param("update_costmap_before_planning", update_costmap_before_planning_, true);
+  if (!nh.param<double>("xy_goal_tolerance", xy_goal_tolerance_, 0.1))
+  {
+    xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1);
+  }
+  if (!nh.param<double>("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1))
+  {
+    yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
+  }
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::reset()
+{
+  robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
+  this->unlock();
+  traj_generator_->reset();
+  goal_checker_->reset();
+  nav_algorithm_->reset();
+  rotate_algorithm_->reset();
+  ret_nav_ = ret_angle_ = false;
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose)
+{
+  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  reset();
+  goal_pose_ = goal_pose;
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::setPlan(const nav_2d_msgs::Path2D &path)
+{
+  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  costmap_robot_->resetLayers();
+  global_plan_ = path;
+  if((unsigned int)global_plan_.poses.size() > 2)
+    global_plan_.poses.erase(global_plan_.poses.begin());
+  for (unsigned int i = 0; i < (unsigned int)global_plan_.poses.size(); i++)
+  {
+    global_plan_.poses[i].header.seq = i;
+  }
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{
+  this->getParams(planner_nh_);
+  if (update_costmap_before_planning_)
+  {
+    info_.width = costmap_->getSizeInCellsX();
+    info_.height = costmap_->getSizeInCellsY();
+    info_.resolution = costmap_->getResolution();
+    info_.frame_id = costmap_robot_->getGlobalFrameID();
+    info_.origin_x = costmap_->getOriginX();
+    info_.origin_y = costmap_->getOriginY();
+
+    if (!costmap_robot_->isCurrent())
+      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+  }
+
+  // Update time stamp of goal pose
+  // goal_pose_.header.stamp = pose.header.stamp;
+  nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
+                             local_goal_pose = this->transformPoseToLocal(goal_pose_);
+
+  if (pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+  {
+    robot::log_debug_at(__FILE__, __LINE__, "Transform global plan is successful");
+  }
+  else
+    robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
+
+  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))
+    {
+      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
+      throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+    }
+  }
+  else
+  {
+    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    {
+      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
+      throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+    }
+  }
+
+  int xytheta_direct[3];
+  xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0;
+  xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0;
+  xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0;
+  traj_generator_->setDirect(xytheta_direct);
+}
+
+nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
+                                                                                          const nav_2d_msgs::Twist2D &velocity)
+{
+  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  nav_2d_msgs::Twist2DStamped cmd_vel;
+  try
+  {
+    if (global_plan_.poses.empty())
+      return cmd_vel;
+    this->prepare(pose, velocity);
+    cmd_vel = this->ScoreAlgorithm(pose, velocity);
+    return cmd_vel;
+  }
+  catch (const nav_core2::PlannerException &e)
+  {
+    throw nav_core2::LocalPlannerException("computing velocity commands has been broken");
+    return cmd_vel;
+  }
+}
+
+nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{
+  nav_2d_msgs::Twist2D twist;
+  mkt_msgs::Trajectory2D traj;
+  nav_2d_msgs::Twist2DStamped cmd_vel;
+  cmd_vel.header.stamp = robot::Time::now();
+
+  if (ret_nav_ && ret_angle_)
+    return cmd_vel;
+  else if (!ret_nav_)
+    traj = nav_algorithm_->calculator(pose, velocity);
+  else
+    traj = rotate_algorithm_->calculator(pose, velocity);
+
+  cmd_vel.velocity = traj.velocity;
+  return cmd_vel;
+}
+
+bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(geometry_msgs::Vector3 linear)
+{
+  try
+  {
+    if (linear.x == 0.0 && linear.y == 0.0 && linear.z == 0.0)
+    {
+      if (nav_algorithm_)
+        nav_algorithm_->stop();
+      if (rotate_algorithm_)
+        rotate_algorithm_->stop();
+    }
+    else
+    {
+      if (nav_algorithm_)
+        nav_algorithm_->resume();
+      if (rotate_algorithm_)
+        rotate_algorithm_->resume();
+    }
+
+    if (traj_generator_)
+      return traj_generator_->setTwistLinear(linear);
+    else
+      throw nav_core2::LocalPlannerException("Failed to set linear");
+  }
+  catch (const std::exception &e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+    return false;
+  }
+}
+
+geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool direct)
+{
+  try
+  {
+    if (traj_generator_)
+      return traj_generator_->getTwistLinear(direct);
+    else
+      throw nav_core2::LocalPlannerException("Failed to get linear");
+  }
+  catch (const std::exception &e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+  }
+}
+
+bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(geometry_msgs::Vector3 angular)
+{
+  try
+  {
+    if (traj_generator_)
+      return traj_generator_->setTwistAngular(angular);
+    else
+      throw nav_core2::LocalPlannerException("Failed to set angular");
+  }
+  catch (const std::exception &e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+    return false;
+  }
+}
+
+geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngular(bool direct)
+{
+  try
+  {
+    if (traj_generator_)
+      return traj_generator_->getTwistAngular(direct);
+    else
+      throw nav_core2::LocalPlannerException("Failed to get angular");
+  }
+  catch (const std::exception &e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+  }
+}
+
+bool pnkx_local_planner::PNKXLocalPlanner::islock()
+{
+  return lock_;
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::lock()
+{
+  lock_ = true;
+}
+
+void pnkx_local_planner::PNKXLocalPlanner::unlock()
+{
+  lock_ = false;
+}
+
+bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
+{
+  if (goal_pose_.header.frame_id == "")
+  {
+    robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
+    return false;
+  }
+  // Update time stamp of goal pose
+  // goal_pose_.header.stamp = pose.header.stamp;
+  nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
+  nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
+  nav_2d_msgs::Path2D plan = transformed_plan_;
+  if (!ret_nav_)
+  {
+    ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity);
+  }
+  else
+  {
+    double delta_orient = fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta));
+    ret_angle_ = (bool)(delta_orient <= yaw_goal_tolerance_);
+  }
+
+  if (ret_nav_ && ret_angle_)
+  {
+    robot::log_info_throttle(1.0, "Goal reached!");
+  }
+  return ret_nav_ && ret_angle_;
+}
+
+inline double getSquareDistance(const nav_2d_msgs::Pose2DStamped &pose_a, const nav_2d_msgs::Pose2DStamped &pose_b)
+{
+  double x_diff = pose_a.pose.x - pose_b.pose.x;
+  double y_diff = pose_a.pose.y - pose_b.pose.y;
+  return x_diff * x_diff + y_diff * y_diff;
+}
+
+nav_2d_msgs::Pose2DStamped pnkx_local_planner::PNKXLocalPlanner::transformPoseToLocal(const nav_2d_msgs::Pose2DStamped &pose)
+{
+  nav_2d_msgs::Pose2DStamped local_pose;
+  nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), pose, local_pose);
+  return local_pose;
+}
+
+nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXLocalPlanner::create()
+{
+  return std::make_shared<PNKXLocalPlanner>();
+}
+
+//  register this planner as a LocalPlanner plugin
+BOOST_DLL_ALIAS(pnkx_local_planner::PNKXLocalPlanner::create, PNKXLocalPlanner)

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

@@ -0,0 +1,234 @@
+#include "pnkx_local_planner/pnkx_rotate_local_planner.h"
+#include "pnkx_local_planner/transforms.h"
+
+#include <nav_core2/exceptions.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+#include <nav_2d_utils/parameters.h>
+#include <mkt_msgs/Trajectory2D.h>
+
+#include <angles/angles.h>
+#include <nav_msgs/Path.h>
+#include <boost/dll/import.hpp>
+#include <boost/dll/alias.hpp>
+
+pnkx_local_planner::PNKXRotateLocalPlanner::PNKXRotateLocalPlanner()
+{
+}
+
+pnkx_local_planner::PNKXRotateLocalPlanner::~PNKXRotateLocalPlanner()
+{
+  if (traj_generator_)
+    traj_generator_.reset();
+
+  if (nav_algorithm_)
+    nav_algorithm_.reset();
+
+  if (goal_checker_)
+    goal_checker_.reset();
+}
+
+void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot)
+{
+  if (!initialized_)
+  {
+    robot::log_info_at(__FILE__, __LINE__, "Using parent name \"%s\"", name.c_str());
+    tf_ = tf;
+    costmap_robot_ = costmap_robot;
+    costmap_ = costmap_robot_->getCostmap();
+
+    info_.width = costmap_->getSizeInCellsX();
+    info_.height = costmap_->getSizeInCellsY();
+    info_.resolution = costmap_->getResolution();
+    info_.frame_id = costmap_robot_->getGlobalFrameID();
+    info_.origin_x = costmap_->getOriginX();
+    info_.origin_y = costmap_->getOriginY();
+
+    parent_ = parent;
+    planner_nh_ = robot::NodeHandle(parent_, name);
+    
+    this->getParams(planner_nh_);
+
+    std::string traj_generator_name;
+    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
+    robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
+        path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
+      traj_generator_ = traj_gen_loader_();
+      if (!traj_generator_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
+      traj_generator_->initialize(nh_traj_gen);
+    }
+    catch (const std::exception& ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
+      exit(1);
+    }
+
+    std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
+    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate"));
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+        path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
+      rotate_algorithm_ = rotate_algorithm_loader_();
+      if (!nav_algorithm_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
+      rotate_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
+    }
+    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_rotate_name.c_str(), ex.what());
+      exit(1);
+    }
+
+    std::string goal_checker_name;
+    planner_nh_.param("goal_checker_name", goal_checker_name, std::string("nav_algorithm::GoalChecker"));
+    try
+    {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
+        path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
+      goal_checker_ = goal_checker_loader_();
+      if (!goal_checker_)
+      {
+        robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
+        exit(1);
+      }
+      robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
+      goal_checker_->initialize(nh_goal_checker);
+    }
+    catch (const std::exception& ex)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
+      exit(1);
+    }
+    
+    robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
+    initialized_ = true;
+  }
+}
+
+void pnkx_local_planner::PNKXRotateLocalPlanner::reset()
+{
+  robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
+  this->unlock();
+  traj_generator_->reset();
+  goal_checker_->reset();
+  nav_algorithm_->reset();
+  ret_nav_ = false;
+}
+
+void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity)
+{
+  this->getParams(planner_nh_);
+  if (update_costmap_before_planning_)
+  {
+    info_.width = costmap_->getSizeInCellsX();
+    info_.height = costmap_->getSizeInCellsY();
+    info_.resolution = costmap_->getResolution();
+    info_.frame_id = costmap_robot_->getGlobalFrameID();
+    info_.origin_x = costmap_->getOriginX();
+    info_.origin_y = costmap_->getOriginY();
+
+    if (!costmap_robot_->isCurrent())
+      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+  }
+
+  // Update time stamp of goal pose
+  // goal_pose_.header.stamp = pose.header.stamp;
+  nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
+    local_goal_pose = this->transformPoseToLocal(goal_pose_);
+
+  try
+  {
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+      throw nav_core2::LocalPlannerException("Transform global plan is failed");
+  }
+  catch(const nav_core2::LocalPlannerException& e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+  }
+
+  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))
+  {
+    throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+  }
+
+  int xytheta_direct[3];
+  xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0;
+  xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0;
+  xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0;
+  traj_generator_->setDirect(xytheta_direct);
+}
+
+nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity)
+{
+  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  nav_2d_msgs::Twist2DStamped cmd_vel;
+  try
+  {
+    if (global_plan_.poses.empty())
+      return cmd_vel;
+    this->prepare(pose, velocity);
+    cmd_vel = this->ScoreAlgorithm(pose, velocity);
+    return cmd_vel;
+  }
+  catch (const nav_core2::PlannerException &e)
+  {
+    throw nav_core2::LocalPlannerException(e.what());
+  }
+}
+
+nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity)
+{
+  nav_2d_msgs::Twist2D twist;
+  mkt_msgs::Trajectory2D traj;
+  nav_2d_msgs::Twist2DStamped cmd_vel;
+  if (ret_nav_)
+    return cmd_vel;
+
+  traj = nav_algorithm_->calculator(pose, velocity);
+  cmd_vel.velocity = traj.velocity;
+  return cmd_vel;
+}
+
+bool pnkx_local_planner::PNKXRotateLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity)
+{
+  if (goal_pose_.header.frame_id == "")
+  {
+    robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
+    return false;
+  }
+  // Update time stamp of goal pose
+  // goal_pose_.header.stamp = pose.header.stamp;
+  nav_2d_msgs::Path2D empty;
+  ret_nav_ = goal_checker_->isGoalReached(
+    this->transformPoseToLocal(pose), this->transformPoseToLocal(goal_pose_), empty, velocity);
+
+  if (ret_nav_)
+    robot::log_info_at(__FILE__, __LINE__, "Goal reached!");
+  return ret_nav_;
+}
+
+nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXRotateLocalPlanner::create()
+{
+  return std::make_shared<pnkx_local_planner::PNKXRotateLocalPlanner>();
+}
+
+//  register this planner as a LocalPlanner plugin
+BOOST_DLL_ALIAS(pnkx_local_planner::PNKXRotateLocalPlanner::create, PNKXRotateLocalPlanner)

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

@@ -0,0 +1,206 @@
+#include <pnkx_local_planner/transforms.h>
+#include <nav_2d_utils/path_ops.h>
+#include <nav_2d_utils/tf_help.h>
+#include <robot/console.h>
+#include <data_convert/data_convert.h>
+#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+
+bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, nav_2d_msgs::Pose2DStamped& global_pose, double transform_tolerance)
+{
+  if (!tf)
+    return false;
+
+  geometry_msgs::PoseStamped global_pose_stamped;
+  tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
+  geometry_msgs::PoseStamped robot_pose;
+  tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
+  robot_pose.header.frame_id = base_frame_id;
+  robot_pose.header.stamp = robot::Time();
+  robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
+  
+  // Convert robot::Time to tf3::Time
+  tf3::Time tf3_current_time = data_convert::convertTime(current_time);
+  tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
+  
+  // get the global pose of the robot
+  try
+  {
+    // use current time if possible (makes sure it's not in the future)
+    std::string error_msg;
+    if (tf->canTransform(global_frame, base_frame_id, tf3_current_time, &error_msg))
+    {
+      // Transform is available at current time
+      tf3::TransformStampedMsg transform = tf->lookupTransform(global_frame, base_frame_id, tf3_current_time);
+      tf3::doTransform(robot_pose, global_pose_stamped, transform);
+    }
+    // use the latest otherwise (tf3::Time() means latest available)
+    else
+    {
+      // Try to get latest transform
+      tf3::TransformStampedMsg transform = tf->lookupTransform(global_frame, base_frame_id, tf3_zero_time);
+      tf3::doTransform(robot_pose, global_pose_stamped, transform);
+    }
+    global_pose = nav_2d_utils::poseStampedToPose2D(global_pose_stamped);
+  }
+  catch (tf3::LookupException& ex)
+  {
+    robot::log_warning_throttle(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
+    return false;
+    // throw nav_core2::LocalPlannerException(ex.what());
+  }
+  catch (tf3::ConnectivityException& ex)
+  {
+    robot::log_warning_throttle(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
+    return false;
+    // throw nav_core2::LocalPlannerException(ex.what());
+  }
+  catch (tf3::ExtrapolationException& ex)
+  {
+    robot::log_warning_throttle(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
+    return false;
+    // throw nav_core2::LocalPlannerException(ex.what());
+  }
+  // check global_pose timeout
+  if (!global_pose_stamped.header.stamp.isZero() && current_time.toSec() - global_pose_stamped.header.stamp.toSec() > transform_tolerance)
+  {
+    robot::log_warning_throttle(1.0, "Local Planner transform %s %s timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", base_frame_id.c_str(), global_frame.c_str(),
+      current_time.toSec(), global_pose_stamped.header.stamp.toSec(), transform_tolerance);
+    return false;
+  }
+  return true;
+}
+
+bool pnkx_local_planner::transformGlobalPose(TFListenerPtr tf, const std::string& global_frame,
+  const nav_2d_msgs::Pose2DStamped& stamped_pose, nav_2d_msgs::Pose2DStamped& newer_pose)
+{
+  if (tf == nullptr)
+    return false;
+  bool result = nav_2d_utils::transformPose(tf, global_frame, stamped_pose, newer_pose);
+  newer_pose.header.seq = stamped_pose.header.seq;
+  return result;
+}
+
+double pnkx_local_planner::getSquareDistance(const geometry_msgs::Pose2D& pose_a, const geometry_msgs::Pose2D& pose_b)
+{
+  double x_diff = pose_a.x - pose_b.x;
+  double y_diff = pose_a.y - pose_b.y;
+  return x_diff * x_diff + y_diff * y_diff;
+}
+
+bool pnkx_local_planner::transformGlobalPlan(
+  TFListenerPtr tf, const nav_2d_msgs::Path2D& global_plan, const nav_2d_msgs::Pose2DStamped& pose,
+  const costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length,
+  nav_2d_msgs::Path2D& transformed_plan, const bool from_global_frame)
+{
+  if (global_plan.poses.size() == 0)
+  {
+    robot::log_warning("Received plan with zero length");
+    throw nav_core2::PlannerException("Received plan with zero length");
+  }
+
+  transformed_plan.poses.clear();
+  if (tf == nullptr)
+    throw nav_core2::PlannerException("TFListenerPtr is null");
+
+  if (global_plan.poses.empty())
+    throw nav_core2::PlannerException("Received plan with zero length");
+
+  try
+  {
+    // let's get the pose of the robot in the frame of the plan
+    nav_2d_msgs::Pose2DStamped robot_pose;
+    if (!nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
+    {
+      throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
+    }
+
+    transformed_plan.header.frame_id = costmap->getGlobalFrameID();
+    transformed_plan.header.stamp = pose.header.stamp;
+
+    nav_2d_msgs::Pose2DStamped new_pose;
+    nav_2d_msgs::Pose2DStamped stamped_pose;
+    stamped_pose.header.frame_id = global_plan.header.frame_id;
+
+    for (unsigned int i = 0; i < global_plan.poses.size(); i++)
+    {
+      // now we'll transform until points are outside of our distance threshold
+      stamped_pose = global_plan.poses[i];
+      if (pnkx_local_planner::transformGlobalPose(tf, global_frame, global_plan.poses[i], new_pose))
+      {
+        transformed_plan.poses.push_back(new_pose);
+      }
+    }
+
+    // Prune both plans based on robot position
+    // Note that this part of the algorithm assumes that the global plan starts near the robot (at one point)
+    // Otherwise it may take a few iterations to converge to the proper behavior
+
+      // let's get the pose of the robot in the frame of the transformed_plan/costmap
+    nav_2d_msgs::Pose2DStamped costmap_pose;
+    if (!nav_2d_utils::transformPose(tf, transformed_plan.header.frame_id, pose, costmap_pose))
+    {
+      throw nav_core2::PlannerTFException("Unable to transform robot pose into costmap's frame");
+    }
+
+    robot::log_assert(global_plan.poses.size() >= transformed_plan.poses.size(), "Global plan size is less than transformed plan size");
+
+    std::vector<nav_2d_msgs::Pose2DStamped>::iterator it = transformed_plan.poses.begin();
+
+    double sq_prune_dist = 0.1;
+
+    std::vector<geometry_msgs::Point> footprint = costmap->getRobotFootprint();
+    size_t n = footprint.size();
+    if (n > 1)
+    {
+      double max_length = 0.0;
+      for (size_t i = 0; i < n; ++i)
+      {
+        const geometry_msgs::Point& p1 = footprint[i];
+        const geometry_msgs::Point& p2 = footprint[(i + 1) % n]; // wrap around to first point
+        double len = std::hypot(p2.x - p1.x, p2.y - p1.y);
+        if (len > max_length)
+        {
+          max_length = len;
+        }
+      }
+      sq_prune_dist += max_length;
+    }
+    // if(from_global_frame) sq_prune_dist = 0;
+    while (it != transformed_plan.poses.end())
+    {
+      const nav_2d_msgs::Pose2DStamped& w = *it;
+      // Fixed error bound of 1 meter for now. Can reduce to a portion of the map size or based on the resolution
+      if (pnkx_local_planner::getSquareDistance(costmap_pose.pose, w.pose) < sq_prune_dist)
+      {
+        robot::log_debug_at(__FILE__, __LINE__, "Nearest waypoint to <%f, %f> is <%f, %f>\n",
+          costmap_pose.pose.x, costmap_pose.pose.y, w.pose.x, w.pose.y);
+        break;
+      }
+
+      it = transformed_plan.poses.erase(it);
+    }
+
+    if (transformed_plan.poses.size() == 0)
+    {
+      throw nav_core2::PlannerTFException("Resulting plan has 0 poses in it.");
+    }
+  }
+  catch (tf3::LookupException& ex)
+  {
+    robot::log_error_at(__FILE__, __LINE__, "No Transform available Error: %s\n", ex.what());
+    return false;
+  }
+  catch (tf3::ConnectivityException& ex)
+  {
+    robot::log_error("Connectivity Error: %s\n", ex.what());
+    return false;
+  }
+  catch (tf3::ExtrapolationException& ex)
+  {
+    robot::log_error("Extrapolation Error: %s\n", ex.what());
+    if (global_plan.poses.size() > 0)
+      robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __FILE__, __LINE__, global_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
+    return false;
+  }
+  return true;
+}

src/Libraries/common_msgs

@@ -1 +1 @@
-Subproject commit 6bac68429824cab4bbb1df3e259a24a8bf741b59
+Subproject commit e7dc4031c674bfa73c800e37e39531a7f828f79d

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit fdfba18bde0662ab5f042b938ecd011c5382ca7a
+Subproject commit b3be5da393abee4ef535f68f702cd84c02f3b98b

src/Libraries/data_convert

@@ -1 +1 @@
-Subproject commit 03a8fa3a802db88ab01d6f7f9694050fd7e74a3a
+Subproject commit 8b22de38ac16eccce348e505e12cfafdf33e2943

src/Libraries/nav_2d_utils/CMakeLists.txt

@@ -32,8 +32,7 @@ target_link_libraries(conversions
         nav_msgs
         nav_grid
         nav_core2
-        robot::node_handle
-        robot::console
+        robot_cpp
     PRIVATE
         console_bridge::console_bridge
         Boost::system
@@ -50,8 +49,7 @@ target_link_libraries(path_ops
     PUBLIC
         nav_2d_msgs
         geometry_msgs
-        robot::node_handle
-        robot::console
+        robot_cpp
 )
 
 add_library(polygons src/polygons.cpp src/footprint.cpp)
@@ -64,8 +62,7 @@ target_link_libraries(polygons
     PUBLIC
         nav_2d_msgs
         geometry_msgs
-        robot::node_handle
-        robot::console
+        robot_cpp
     PRIVATE
         ${xmlrpcpp_LIBRARIES}
 )
@@ -87,8 +84,7 @@ target_link_libraries(bounds
     PUBLIC
         nav_grid
         nav_core2
-        robot::node_handle
-        robot::console
+        robot_cpp
 )
 
 add_library(tf_help src/tf_help.cpp)
@@ -103,8 +99,7 @@ target_link_libraries(tf_help
         geometry_msgs
         nav_core2
         tf3
-        robot::node_handle
-        robot::console
+        robot_cpp
 )
 
 # Create an INTERFACE library that represents all nav_2d_utils libraries
@@ -121,8 +116,7 @@ target_link_libraries(nav_2d_utils
         polygons
         bounds
         tf_help
-        robot::node_handle
-        robot::console
+        robot_cpp
 )
 
 # Install header files

src/Libraries/nav_2d_utils/doc/Conversions.md

@@ -17,7 +17,7 @@
 | to `nav_2d_msgs` | from `nav_2d_msgs` |
 | -- | -- |
 | `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
-||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, ros::Time)`|
+||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`|
 ||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`|
 | `Pose2DStamped stampedPoseToPose2D(tf::Stamped<tf::Pose>)` | |
 
@@ -25,7 +25,7 @@
 | to `nav_2d_msgs` | from `nav_2d_msgs` |
 | -- | -- |
 | `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)`
-| `Path2D posesToPath2D(std::vector<geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<geometry_msgs::Pose2D>, std::string, ros::Time)`
+| `Path2D posesToPath2D(std::vector<geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<geometry_msgs::Pose2D>, std::string, robot::Time)`
 
 Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
 

src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h

@@ -35,7 +35,10 @@
 #ifndef NAV_2D_UTILS_PARAMETERS_H
 #define NAV_2D_UTILS_PARAMETERS_H
 
+#include <robot/node_handle.h>
+#include <robot/console.h>
 #include <string>
+#include <iostream>
 
 namespace nav_2d_utils
 {
@@ -55,12 +58,13 @@ template<class param_t>
 param_t searchAndGetParam(const robot::NodeHandle& nh, const std::string& param_name, const param_t& default_value)
 {
   std::string resolved_name;
-  // if (nh.searchParam(param_name, resolved_name))
-  // {
-  //   param_t value = default_value;
-  //   nh.param(resolved_name, value, default_value);
-  //   return value;
-  // }
+  if (nh.searchParam(param_name, resolved_name))
+  {
+    param_t value = default_value;
+    robot::NodeHandle nh_private = robot::NodeHandle("~");
+    nh_private.param(resolved_name, value, default_value);
+    return value;
+  }
   return default_value;
 }
 
@@ -84,7 +88,7 @@ param_t loadParameterWithDeprecation(const robot::NodeHandle& nh, const std::str
   }
   if (nh.hasParam(old_name))
   {
-    std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
+    robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
     nh.getParam(old_name, value, default_value);
     return value;
   }
@@ -103,7 +107,7 @@ void moveDeprecatedParameter(const robot::NodeHandle& nh, const std::string curr
   if (!nh.hasParam(old_name)) return;
 
   param_t value;
-  std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
+  robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
   value = nh.param<param_t>(old_name);
   nh.setParam(current_name, value);
 }

src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h

@@ -205,8 +205,8 @@ protected:
   std::string current_plugin_;
 
   // ROS Interface
-  ros::ServiceServer switch_plugin_srv_;
-  ros::Publisher current_plugin_pub_;
+  robot::ServiceServer switch_plugin_srv_;
+  robot::Publisher current_plugin_pub_;
   robot::NodeHandle private_nh_;
   std::string ros_name_;
 

src/Libraries/nav_2d_utils/src/conversions.cpp

@@ -128,7 +128,7 @@ geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped&
 }
 
 // geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d,
-//                                                const std::string& frame, const ros::Time& stamp)
+//                                                const std::string& frame, const robot::Time& stamp)
 // {
 //   geometry_msgs::PoseStamped pose;
 //   pose.header.frame_id = frame;
@@ -189,7 +189,7 @@ nav_2d_msgs::Path2D posesToPath2D(const std::vector<geometry_msgs::PoseStamped>&
 
 
 // nav_msgs::Path poses2DToPath(const std::vector<geometry_msgs::Pose2D>& poses,
-//                              const std::string& frame, const ros::Time& stamp)
+//                              const std::string& frame, const robot::Time& stamp)
 // {
 //   nav_msgs::Path path;
 //   path.poses.resize(poses.size());

src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py

@@ -1,131 +0,0 @@
-from geometry_msgs.msg import Pose, Pose2D, Quaternion, Point
-from nav_2d_msgs.msg import Twist2D, Path2D, Pose2DStamped, Point2D
-from nav_msgs.msg import Path
-from geometry_msgs.msg import Twist, PoseStamped
-
-try:
-    from tf.transformations import euler_from_quaternion, quaternion_from_euler
-
-    def get_yaw(orientation):
-        rpy = euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w))
-        return rpy[2]
-
-    def from_yaw(yaw):
-        q = Quaternion()
-        q.x, q.y, q.z, q.w = quaternion_from_euler(0, 0, yaw)
-        return q
-except ImportError:
-    from math import sin, cos, atan2
-    # From https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
-
-    def from_yaw(yaw):
-        q = Quaternion()
-        q.z = sin(yaw * 0.5)
-        q.w = cos(yaw * 0.5)
-        return q
-
-    def get_yaw(q):
-        siny_cosp = +2.0 * (q.w * q.z + q.x * q.y)
-        cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z)
-        return atan2(siny_cosp, cosy_cosp)
-
-
-def twist2Dto3D(cmd_vel_2d):
-    cmd_vel = Twist()
-    cmd_vel.linear.x = cmd_vel_2d.x
-    cmd_vel.linear.y = cmd_vel_2d.y
-    cmd_vel.angular.z = cmd_vel_2d.theta
-    return cmd_vel
-
-
-def twist3Dto2D(cmd_vel):
-    cmd_vel_2d = Twist2D()
-    cmd_vel_2d.x = cmd_vel.linear.x
-    cmd_vel_2d.y = cmd_vel.linear.y
-    cmd_vel_2d.theta = cmd_vel.angular.z
-    return cmd_vel_2d
-
-
-def pointToPoint3D(point_2d):
-    point = Point()
-    point.x = point_2d.x
-    point.y = point_2d.y
-    return point
-
-
-def pointToPoint2D(point):
-    point_2d = Point2D()
-    point_2d.x = point.x
-    point_2d.y = point.y
-    return point_2d
-
-
-def poseToPose2D(pose):
-    pose2d = Pose2D()
-    pose2d.x = pose.position.x
-    pose2d.y = pose.position.y
-    pose2d.theta = get_yaw(pose.orientation)
-    return pose2d
-
-
-def poseStampedToPose2DStamped(pose):
-    pose2d = Pose2DStamped()
-    pose2d.header = pose.header
-    pose2d.pose = poseToPose2D(pose.pose)
-    return pose2d
-
-
-def poseToPose2DStamped(pose, frame, stamp):
-    pose2d = Pose2DStamped()
-    pose2d.header.frame_id = frame
-    pose2d.header.stamp = stamp
-    pose2d.pose = poseToPose2D(pose)
-    return pose2d
-
-
-def pose2DToPose(pose2d):
-    pose = Pose()
-    pose.position.x = pose2d.x
-    pose.position.y = pose2d.y
-    pose.orientation = from_yaw(pose2d.theta)
-    return pose
-
-
-def pose2DStampedToPoseStamped(pose2d):
-    pose = PoseStamped()
-    pose.header = pose2d.header
-    pose.pose = pose2DToPose(pose2d.pose)
-    return pose
-
-
-def pose2DToPoseStamped(pose2d, frame, stamp):
-    pose = PoseStamped()
-    pose.header.frame_id = frame
-    pose.header.stamp = stamp
-    pose.pose.position.x = pose2d.x
-    pose.pose.position.y = pose2d.y
-    pose.pose.orientation = from_yaw(pose2d.theta)
-    return pose
-
-
-def pathToPath2D(path):
-    path2d = Path2D()
-    if len(path.poses) == 0:
-        return path
-    path2d.header.frame_id = path.poses[0].header.frame_id
-    path2d.header.stamp = path.poses[0].header.stamp
-    for pose in path.poses:
-        stamped = poseStampedToPose2DStamped(pose)
-        path2d.poses.append(stamped.pose)
-    return path2d
-
-
-def path2DToPath(path2d):
-    path = Path()
-    path.header = path2d.header
-    for pose2d in path2d.poses:
-        pose = PoseStamped()
-        pose.header = path2d.header
-        pose.pose = pose2DToPose(pose2d)
-        path.poses.append(pose)
-    return path

src/Libraries/nav_2d_utils/src/polygons.cpp

@@ -37,6 +37,7 @@
 #include <mapbox/earcut.hpp>
 #include <algorithm>
 #include <limits>
+#include <sstream>
 #include <string>
 #include <vector>
 

src/Libraries/nav_2d_utils/test/param_tests.cpp

@@ -140,7 +140,7 @@ TEST(Polygon2D, test_write)
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "param_tests");
+  robot::init(argc, argv, "param_tests");
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }

src/Libraries/robot_cpp

@@ -1 +1 @@
-Subproject commit 1974d0ddeea08a05b83363614d1a946db5dd07ee
+Subproject commit 5b23baae3ace0f5acd7dc7e9cd346ffc75210630

src/Libraries/tf3/CHANGELOG.rst

@@ -309,7 +309,7 @@ Changelog for package tf2
 ------------------
 * splitting rospy dependency into tf2_py so tf2 is pure c++ library.
 * switching to console_bridge from rosconsole
-* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2
+* moving convert methods back into tf2 because it does not have any ros dependencies beyond robot::Time which is already a dependency of tf2
 * Cleaning up unnecessary dependency on roscpp
 * Cleaning up packaging of tf2 including:
   removing unused nodehandle

src/Libraries/tf3/include/tf3/buffer_core.h

@@ -166,7 +166,7 @@ public:
   geometry_msgs::Twist
     lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
 		const tf::Point & reference_point, const std::string& reference_point_frame, 
-		const ros::Time& time, const ros::Duration& averaging_interval) const;
+		const robot::Time& time, const robot::Duration& averaging_interval) const;
   */
   /* \brief lookup the twist of the tracking frame with respect to the observational frame
    * 
@@ -183,7 +183,7 @@ public:
   /*
   geometry_msgs::Twist
     lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, 
-		const ros::Time& time, const ros::Duration& averaging_interval) const;
+		const robot::Time& time, const robot::Duration& averaging_interval) const;
   */
   /** \brief Test if a transform is possible
    * \param target_frame The frame into which to transform

src/Libraries/tf3/include/tf3/compat.h

@@ -1,4 +1,4 @@
-// Compatibility types to avoid using ros:: or geometry_msgs:: namespaces inside tf3
+// Compatibility types to avoid using robot:: or geometry_msgs:: namespaces inside tf3
 #ifndef TF3_COMPAT_H
 #define TF3_COMPAT_H
 
@@ -7,7 +7,7 @@
 
 namespace tf3 {
 
-// Minimal header/message equivalents owned by tf3 (no ros:: or geometry_msgs::)
+// Minimal header/message equivalents owned by tf3 (no robot:: or geometry_msgs::)
 struct HeaderMsg
 {
   uint32_t seq;

src/Libraries/tf3/include/tf3/exceptions.h

@@ -37,7 +37,7 @@
 namespace tf3{
 
 /** \brief A base class for all tf3 exceptions 
- * This inherits from ros::exception 
+ * This inherits from robot::exception 
  * which inherits from std::runtime_exception
  */
 class TransformException: public std::runtime_error

src/Libraries/tf3/src/buffer_core.cpp

@@ -674,8 +674,8 @@ tf3::TransformStampedMsg BufferCore::lookupTransform(const std::string& target_f
 /*
 geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
                                           const std::string& observation_frame, 
-                                          const ros::Time& time, 
-                                          const ros::Duration& averaging_interval) const
+                                          const robot::Time& time, 
+                                          const robot::Duration& averaging_interval) const
 {
   try
   {
@@ -707,8 +707,8 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
                                           const std::string& reference_frame,
                                           const tf3::Point & reference_point, 
                                           const std::string& reference_point_frame, 
-                                          const ros::Time& time, 
-                                          const ros::Duration& averaging_interval) const
+                                          const robot::Time& time, 
+                                          const robot::Duration& averaging_interval) const
 {
   try{
   geometry_msgs::Twist t;

src/Libraries/tf3/test/cache_unittest.cpp

@@ -82,7 +82,7 @@ TEST(TimeCache, Repeatability)
   for ( uint64_t i = 1; i < runs ; i++ )
   {
     stor.frame_id_ = i;
-    stor.stamp_ = ros::Time().fromNSec(i);
+    stor.stamp_ = robot::Time().fromNSec(i);
     
     cache.insertData(stor);
   }
@@ -90,9 +90,9 @@ TEST(TimeCache, Repeatability)
   for ( uint64_t i = 1; i < runs ; i++ )
 
   {
-    cache.getData(ros::Time().fromNSec(i), stor);
+    cache.getData(robot::Time().fromNSec(i), stor);
     EXPECT_EQ(stor.frame_id_, i);
-    EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
   }
   
 }
@@ -109,16 +109,16 @@ TEST(TimeCache, RepeatabilityReverseInsertOrder)
   for ( int i = runs -1; i >= 0 ; i-- )
   {
     stor.frame_id_ = i;
-    stor.stamp_ = ros::Time().fromNSec(i);
+    stor.stamp_ = robot::Time().fromNSec(i);
     
     cache.insertData(stor);
   }
   for ( uint64_t i = 1; i < runs ; i++ )
 
   {
-    cache.getData(ros::Time().fromNSec(i), stor);
+    cache.getData(robot::Time().fromNSec(i), stor);
     EXPECT_EQ(stor.frame_id_, i);
-    EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
   }
   
 }
@@ -143,16 +143,16 @@ TEST(TimeCache, RepeatabilityRandomInsertOrder)
     ss << values[i];
     stor.header.frame_id = ss.str();
     stor.frame_id_ = i;
-    stor.stamp_ = ros::Time().fromNSec(i);
+    stor.stamp_ = robot::Time().fromNSec(i);
     
     cache.insertData(stor);
   }
   for ( uint64_t i = 1; i < runs ; i++ )
 
   {
-    cache.getData(ros::Time().fromNSec(i), stor);
+    cache.getData(robot::Time().fromNSec(i), stor);
     EXPECT_EQ(stor.frame_id_, i);
-    EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
     std::stringstream ss;
     ss << values[i];
     EXPECT_EQ(stor.header.frame_id, ss.str());
@@ -173,36 +173,36 @@ TEST(TimeCache, ZeroAtFront)
   for ( uint64_t i = 1; i < runs ; i++ )
   {
     stor.frame_id_ = i;
-    stor.stamp_ = ros::Time().fromNSec(i);
+    stor.stamp_ = robot::Time().fromNSec(i);
     
     cache.insertData(stor);
   }
 
   stor.frame_id_ = runs;
-  stor.stamp_ = ros::Time().fromNSec(runs);
+  stor.stamp_ = robot::Time().fromNSec(runs);
   cache.insertData(stor);
 
   for ( uint64_t i = 1; i < runs ; i++ )
 
   {
-    cache.getData(ros::Time().fromNSec(i), stor);
+    cache.getData(robot::Time().fromNSec(i), stor);
     EXPECT_EQ(stor.frame_id_, i);
-    EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
   }
 
-  cache.getData(ros::Time(), stor);
+  cache.getData(robot::Time(), stor);
   EXPECT_EQ(stor.frame_id_, runs);
-  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
+  EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(runs));
 
   stor.frame_id_ = runs;
-  stor.stamp_ = ros::Time().fromNSec(runs+1);
+  stor.stamp_ = robot::Time().fromNSec(runs+1);
   cache.insertData(stor);
 
 
   //Make sure we get a different value now that a new values is added at the front
-  cache.getData(ros::Time(), stor);
+  cache.getData(robot::Time(), stor);
   EXPECT_EQ(stor.frame_id_, runs);
-  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
+  EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(runs+1));
   
 }
 
@@ -233,14 +233,14 @@ TEST(TimeCache, CartesianInterpolation)
     
       stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
       stor.frame_id_ = 2;
-      stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
+      stor.stamp_ = robot::Time().fromNSec(step * 100 + offset);
       cache.insertData(stor);
     }
     
     for (int pos = 0; pos < 100 ; pos ++)
     {
       uint64_t time = offset + pos;
-      cache.getData(ros::Time().fromNSec(time), stor);
+      cache.getData(robot::Time().fromNSec(time), stor);
       uint64_t time_out = stor.stamp_.toNSec();
       double x_out = stor.translation_.x();
       double y_out = stor.translation_.y();
@@ -286,13 +286,13 @@ TEST(TimeCache, ReparentingInterpolationProtection)
 
     stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
     stor.frame_id_ = step + 4;
-    stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
+    stor.stamp_ = robot::Time().fromNSec(step * 100 + offset);
     cache.insertData(stor);
   }
   
   for (int pos = 0; pos < 100 ; pos ++)
   {
-    EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor));
+    EXPECT_TRUE(cache.getData(robot::Time().fromNSec(offset + pos), stor));
     double x_out = stor.translation_.x();
     double y_out = stor.translation_.y();
     double z_out = stor.translation_.z();
@@ -354,14 +354,14 @@ TEST(TimeCache, AngularInterpolation)
       quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
       stor.rotation_ = quats[step];
       stor.frame_id_ = 3;
-      stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
+      stor.stamp_ = robot::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
       cache.insertData(stor);
     }
     
     for (int pos = 0; pos < 100 ; pos ++)
     {
       uint64_t time = offset + pos;
-      cache.getData(ros::Time().fromNSec(time), stor);
+      cache.getData(robot::Time().fromNSec(time), stor);
       uint64_t time_out = stor.stamp_.toNSec();
       tf3::Quaternion quat (stor.rotation_);
 
@@ -388,14 +388,14 @@ TEST(TimeCache, DuplicateEntries)
   TransformStorage stor;
   setIdentity(stor);
   stor.frame_id_ = 3;
-  stor.stamp_ = ros::Time().fromNSec(1);
+  stor.stamp_ = robot::Time().fromNSec(1);
 
   cache.insertData(stor);
 
   cache.insertData(stor);
 
 
-  cache.getData(ros::Time().fromNSec(1), stor);
+  cache.getData(robot::Time().fromNSec(1), stor);
   
   //printf(" stor is %f\n", stor.translation_.x());
   EXPECT_TRUE(!std::isnan(stor.translation_.x()));

src/Libraries/tf3/test/simple_tf3_core.cpp

@@ -46,7 +46,7 @@ TEST(tf3, setTransformValid)
   tf3::BufferCore tfc;
   geometry_msgs::TransformStamped st;
   st.header.frame_id = "foo";
-  st.header.stamp = ros::Time(1.0);
+  st.header.stamp = robot::Time(1.0);
   st.child_frame_id = "child";
   st.transform.rotation.w = 1;
   EXPECT_TRUE(tfc.setTransform(st, "authority1"));
@@ -58,7 +58,7 @@ TEST(tf3, setTransformInvalidQuaternion)
   tf3::BufferCore tfc;
   geometry_msgs::TransformStamped st;
   st.header.frame_id = "foo";
-  st.header.stamp = ros::Time(1.0);
+  st.header.stamp = robot::Time(1.0);
   st.child_frame_id = "child";
   st.transform.rotation.w = 0;
   EXPECT_FALSE(tfc.setTransform(st, "authority1"));
@@ -68,17 +68,17 @@ TEST(tf3, setTransformInvalidQuaternion)
 TEST(tf3_lookupTransform, LookupException_Nothing_Exists)
 {
   tf3::BufferCore tfc;
-  EXPECT_THROW(tfc.lookupTransform("a", "b", ros::Time().fromSec(1.0)), tf3::LookupException);
+  EXPECT_THROW(tfc.lookupTransform("a", "b", robot::Time().fromSec(1.0)), tf3::LookupException);
 
 }
 
 TEST(tf3_canTransform, Nothing_Exists)
 {
   tf3::BufferCore tfc;
-  EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0)));
+  EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0)));
 
   std::string error_msg = std::string();
-  EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0), &error_msg));
+  EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0), &error_msg));
   ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist.");
 
 }
@@ -88,11 +88,11 @@ TEST(tf3_lookupTransform, LookupException_One_Exists)
   tf3::BufferCore tfc;
   geometry_msgs::TransformStamped st;
   st.header.frame_id = "foo";
-  st.header.stamp = ros::Time(1.0);
+  st.header.stamp = robot::Time(1.0);
   st.child_frame_id = "child";
   st.transform.rotation.w = 1;
   EXPECT_TRUE(tfc.setTransform(st, "authority1"));
-  EXPECT_THROW(tfc.lookupTransform("foo", "bar", ros::Time().fromSec(1.0)), tf3::LookupException);
+  EXPECT_THROW(tfc.lookupTransform("foo", "bar", robot::Time().fromSec(1.0)), tf3::LookupException);
 
 }
 
@@ -101,11 +101,11 @@ TEST(tf3_canTransform, One_Exists)
   tf3::BufferCore tfc;
   geometry_msgs::TransformStamped st;
   st.header.frame_id = "foo";
-  st.header.stamp = ros::Time(1.0);
+  st.header.stamp = robot::Time(1.0);
   st.child_frame_id = "child";
   st.transform.rotation.w = 1;
   EXPECT_TRUE(tfc.setTransform(st, "authority1"));
-  EXPECT_FALSE(tfc.canTransform("foo", "bar", ros::Time().fromSec(1.0)));
+  EXPECT_FALSE(tfc.canTransform("foo", "bar", robot::Time().fromSec(1.0)));
 }
 
 TEST(tf3_chainAsVector, chain_v_configuration)
@@ -114,7 +114,7 @@ TEST(tf3_chainAsVector, chain_v_configuration)
   tf3::TestBufferCore tBC;
 
   geometry_msgs::TransformStamped st;
-  st.header.stamp = ros::Time(0);
+  st.header.stamp = robot::Time(0);
   st.transform.rotation.w = 1;
 
   st.header.frame_id = "a";
@@ -136,34 +136,34 @@ TEST(tf3_chainAsVector, chain_v_configuration)
   std::vector<std::string> chain;
   
   
-  mBC._chainAsVector( "c", ros::Time(), "c", ros::Time(), "c", chain);
+  mBC._chainAsVector( "c", robot::Time(), "c", robot::Time(), "c", chain);
   EXPECT_EQ( 0, chain.size());
 
-  mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "c", chain);
+  mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "c", chain);
   EXPECT_EQ( 3, chain.size());
   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
 
-  mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "c", chain);
+  mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "c", chain);
   EXPECT_EQ( 3, chain.size());
   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
 
-  mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "a", chain);
+  mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "a", chain);
   EXPECT_EQ( 3, chain.size());
   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
 
-  mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "a", chain);
+  mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "a", chain);
   EXPECT_EQ( 3, chain.size());
   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
   
-  mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "c", chain);
+  mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "c", chain);
 
   EXPECT_EQ( 5, chain.size());
   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
@@ -172,7 +172,7 @@ TEST(tf3_chainAsVector, chain_v_configuration)
   if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
   if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
 
-  mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "a", chain);
+  mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "a", chain);
 
   EXPECT_EQ( 5, chain.size());
   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
@@ -181,7 +181,7 @@ TEST(tf3_chainAsVector, chain_v_configuration)
   if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
   if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
 
-  mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "e", chain);
+  mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "e", chain);
 
   EXPECT_EQ( 5, chain.size());
   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
@@ -197,7 +197,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration)
   tf3::TestBufferCore tBC;
 
   geometry_msgs::TransformStamped st;
-  st.header.stamp = ros::Time(0);
+  st.header.stamp = robot::Time(0);
   st.transform.rotation.w = 1;
 
   st.header.frame_id = "a";
@@ -217,7 +217,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration)
   mBC.setTransform(st, "authority1");
 
   std::vector<tf3::CompactFrameID> id_chain;
-  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
+  tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
 
   EXPECT_EQ(5, id_chain.size() );
   if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
@@ -227,7 +227,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration)
   if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4]));
 
   id_chain.clear();
-  tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
 
   EXPECT_EQ(5, id_chain.size() );
   if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
@@ -244,7 +244,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
   tf3::TestBufferCore tBC;
 
   geometry_msgs::TransformStamped st;
-  st.header.stamp = ros::Time(0);
+  st.header.stamp = robot::Time(0);
   st.transform.rotation.w = 1;
 
   // st.header.frame_id = "aaa";
@@ -272,7 +272,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
   mBC.setTransform(st, "authority1");
 
   std::vector<tf3::CompactFrameID> id_chain;
-  tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain);
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain);
 
   EXPECT_EQ(5, id_chain.size());
   if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0]));
@@ -281,7 +281,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
   if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
   if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
 
-  tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain);
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain);
   EXPECT_EQ(5, id_chain.size());
   if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
@@ -289,24 +289,24 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
   if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
   if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4]));
 
-  tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
   EXPECT_EQ( id_chain.size(), 3 );
   if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
   if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
 
-  tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
   EXPECT_EQ( id_chain.size(), 3 );
   if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
   if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2]));
 
-  tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain);
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain);
   EXPECT_EQ( id_chain.size(), 2 );
   if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0]));
   if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1]));
 
-  tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain);
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain);
   EXPECT_EQ( id_chain.size(), 2 );
   if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
@@ -315,6 +315,6 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
 
 int main(int argc, char **argv){
   testing::InitGoogleTest(&argc, argv);
-  ros::Time::init(); //needed for ros::TIme::now()
+  robot::Time::init(); //needed for robot::TIme::now()
   return RUN_ALL_TESTS();
 }

src/Libraries/tf3/test/speed_test.cpp

@@ -51,13 +51,13 @@ int main(int argc, char** argv)
 
   tf3::BufferCore bc;
   geometry_msgs::TransformStamped t;
-  t.header.stamp = ros::Time(1);
+  t.header.stamp = robot::Time(1);
   t.header.frame_id = "root";
   t.child_frame_id = "0";
   t.transform.translation.x = 1;
   t.transform.rotation.w = 1.0;
   bc.setTransform(t, "me");
-  t.header.stamp = ros::Time(2);
+  t.header.stamp = robot::Time(2);
   bc.setTransform(t, "me");
 
   for (uint32_t i = 1; i < num_levels / 2; ++i)
@@ -69,7 +69,7 @@ int main(int argc, char** argv)
       std::stringstream child_ss;
       child_ss << i;
 
-      t.header.stamp = ros::Time(j);
+      t.header.stamp = robot::Time(j);
       t.header.frame_id = parent_ss.str();
       t.child_frame_id = child_ss.str();
       bc.setTransform(t, "me");
@@ -79,10 +79,10 @@ int main(int argc, char** argv)
   t.header.frame_id = "root";
   std::stringstream ss;
   ss << num_levels/2;
-  t.header.stamp = ros::Time(1);
+  t.header.stamp = robot::Time(1);
   t.child_frame_id = ss.str();
   bc.setTransform(t, "me");
-  t.header.stamp = ros::Time(2);
+  t.header.stamp = robot::Time(2);
   bc.setTransform(t, "me");
 
   for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
@@ -94,7 +94,7 @@ int main(int argc, char** argv)
       std::stringstream child_ss;
       child_ss << i;
 
-      t.header.stamp = ros::Time(j);
+      t.header.stamp = robot::Time(j);
       t.header.frame_id = parent_ss.str();
       t.child_frame_id = child_ss.str();
       bc.setTransform(t, "me");
@@ -113,13 +113,13 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(0));
+      out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(0));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     //ROS_INFO_STREAM(out_t);
     CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
@@ -127,13 +127,13 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1));
+      out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     //ROS_INFO_STREAM(out_t);
     CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
@@ -141,13 +141,13 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5));
+      out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1.5));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     //ROS_INFO_STREAM(out_t);
     CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
@@ -155,13 +155,13 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(2));
+      out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(2));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     //ROS_INFO_STREAM(out_t);
     CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
@@ -169,13 +169,13 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      bc.canTransform(v_frame1, v_frame0, ros::Time(0));
+      bc.canTransform(v_frame1, v_frame0, robot::Time(0));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     //ROS_INFO_STREAM(out_t);
     CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
@@ -183,13 +183,13 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      bc.canTransform(v_frame1, v_frame0, ros::Time(1));
+      bc.canTransform(v_frame1, v_frame0, robot::Time(1));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     //ROS_INFO_STREAM(out_t);
     CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
@@ -197,13 +197,13 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      bc.canTransform(v_frame1, v_frame0, ros::Time(1.5));
+      bc.canTransform(v_frame1, v_frame0, robot::Time(1.5));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     //ROS_INFO_STREAM(out_t);
     CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
@@ -211,13 +211,13 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      bc.canTransform(v_frame1, v_frame0, ros::Time(2));
+      bc.canTransform(v_frame1, v_frame0, robot::Time(2));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     //ROS_INFO_STREAM(out_t);
     CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
@@ -225,27 +225,27 @@ int main(int argc, char** argv)
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     for (uint32_t i = 0; i < count; ++i)
     {
-      bc.canTransform(v_frame1, v_frame0, ros::Time(3));
+      bc.canTransform(v_frame1, v_frame0, robot::Time(3));
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     CONSOLE_BRIDGE_logInform("canTransform at Time(3) without error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
 #endif
 
 #if 01
   {
-    ros::WallTime start = ros::WallTime::now();
+    robot::WallTime start = robot::WallTime::now();
     std::string str;
     for (uint32_t i = 0; i < count; ++i)
     {
-      bc.canTransform(v_frame1, v_frame0, ros::Time(3), &str);
+      bc.canTransform(v_frame1, v_frame0, robot::Time(3), &str);
     }
-    ros::WallTime end = ros::WallTime::now();
-    ros::WallDuration dur = end - start;
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
     CONSOLE_BRIDGE_logInform("canTransform at Time(3) with error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
   }
 #endif

src/Libraries/tf3/test/static_cache_test.cpp

@@ -56,14 +56,14 @@ TEST(StaticCache, Repeatability)
   for ( uint64_t i = 1; i < runs ; i++ )
   {
     stor.frame_id_ = CompactFrameID(i);
-    stor.stamp_ = ros::Time().fromNSec(i);
+    stor.stamp_ = robot::Time().fromNSec(i);
     
     cache.insertData(stor);
 
     
-    cache.getData(ros::Time().fromNSec(i), stor);
+    cache.getData(robot::Time().fromNSec(i), stor);
     EXPECT_EQ(stor.frame_id_, i);
-    EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
     
   }
 }
@@ -76,14 +76,14 @@ TEST(StaticCache, DuplicateEntries)
   TransformStorage stor;
   setIdentity(stor);
   stor.frame_id_ = CompactFrameID(3);
-  stor.stamp_ = ros::Time().fromNSec(1);
+  stor.stamp_ = robot::Time().fromNSec(1);
 
   cache.insertData(stor);
 
   cache.insertData(stor);
 
 
-  cache.getData(ros::Time().fromNSec(1), stor);
+  cache.getData(robot::Time().fromNSec(1), stor);
   
   //printf(" stor is %f\n", stor.transform.translation.x);
   EXPECT_TRUE(!std::isnan(stor.translation_.x()));

src/Libraries/tf3/test/test_transform_datatypes.cpp

@@ -35,8 +35,8 @@
 
 TEST(Stamped, assignment)
 {
-  tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
-  tf3::Stamped<std::string> second("baz", ros::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> second("baz", robot::Time(0), "my_frame_id");
 
   EXPECT_NE(second, first);
   second = first;
@@ -45,8 +45,8 @@ TEST(Stamped, assignment)
 
 TEST(Stamped, setData)
 {
-  tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
-  tf3::Stamped<std::string> second("baz", ros::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> second("baz", robot::Time(0), "my_frame_id");
 
   EXPECT_NE(second, first);
   second.setData("foobar");
@@ -55,7 +55,7 @@ TEST(Stamped, setData)
 
 TEST(Stamped, copy_constructor)
 {
-  tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
   tf3::Stamped<std::string> second(first);
 
   EXPECT_EQ(second, first);
@@ -63,7 +63,7 @@ TEST(Stamped, copy_constructor)
 
 TEST(Stamped, default_constructor)
 {
-  tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
   tf3::Stamped<std::string> second;
 
   EXPECT_NE(second, first);
@@ -72,6 +72,6 @@ TEST(Stamped, default_constructor)
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  ros::Time::init();
+  robot::Time::init();
   return RUN_ALL_TESTS();
 }

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

@@ -136,7 +136,7 @@ namespace nav_core
      * @brief  Constructs the local planner
      * @param name The name to give this instance of the local planner
      * @param tf A pointer to a transform listener
-     * @param costmap_ros The cost map to use for assigning costs to local plans
+     * @param costmap_robot The cost map to use for assigning costs to local plans
      */
     virtual void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
 

src/Navigations/Cores/nav_core2/CMakeLists.txt

@@ -28,8 +28,7 @@ target_link_libraries(nav_core2 INTERFACE
         tf3 
         nav_grid 
         nav_2d_msgs
-        robot::node_handle
-        robot::console
+        robot_cpp
 )
 
 # Set include directories

src/Navigations/Cores/nav_core2/README.md

@@ -3,15 +3,15 @@ A replacement interface for [nav_core](https://github.com/ros-planning/navigatio
 
 There were a few key reasons for creating new interfaces rather than extending the old ones.
  * Use `nav_2d_msgs` to eliminate unused data fields
- * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROS`.
+ * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROBOT`.
  * Provide more data in the interfaces for easier testing.
  * Use Exceptions rather than booleans to provide more information about the types of errors encountered.
 
 ## `Costmap`
-`costmap_2d::Costmap2DROS` has been a vital part of the navigation stack for years, but it was not without its flaws.
- * Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROS`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested.
+`costmap_2d::Costmap2DROBOT` has been a vital part of the navigation stack for years, but it was not without its flaws.
+ * Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROBOT`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested.
  * Initialization also started an update thread, which is also not always needed in testing.
- * Using `Costmap2DROS` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
+ * Using `Costmap2DROBOT` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
 
 The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `nav_grid::NavGrid<unsigned char>` for abstracting away the data storage and coordinate manipulation, and provides a few other key methods for costmap functioning such as
  * a mutex
@@ -20,7 +20,7 @@ The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `n
 
 The `Costmap` can be loaded using `pluginlib`, allowing for arbitrary implementations of underlying update algorithms, include the layered costmap approach.
 
-One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROS`.
+One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROBOT`.
 
 Note: One could also imagine the `Costmap` interface being templatized itself like `NavGrid` instead of forcing use of `unsigned char`. While this may be possible, it was decided that it was a bit ambitious and the use of templates would force all of the implementation into headers, and would ultimately obfuscate the operation of algorithms.
 
@@ -29,7 +29,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
 
 | `nav_core` | `nav_core2` | comments |
 |---|--|---|
-|`void initialize(std::string, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
+|`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
 |`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
 
 ## Local Planner
@@ -37,7 +37,7 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
 
 | `nav_core` | `nav_core2` | comments |
 |---|--|---|
-|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
+|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
 |(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
 |`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const nav_2d_msgs::Path2D&)`||
 |`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|

src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h

@@ -137,7 +137,7 @@ public:
  * @class CostmapDataLagException
  * @brief Indicates costmap is out of date because data in not up to date.
  *
- * Functions similarly to `!Costmap2DROS::isCurrent()`
+ * Functions similarly to `!Costmap2Drobot::isCurrent()`
  */
 class CostmapDataLagException: public CostmapSafetyException
 {

src/Navigations/Cores/nav_core_adapter/CMakeLists.txt

@@ -33,6 +33,7 @@ target_link_libraries(local_planner_adapter
     Boost::system
     dl               
     costmap_adapter 
+    robot_cpp
     ${PACKAGES_DIR}
 )
 target_include_directories(local_planner_adapter PRIVATE
@@ -53,6 +54,33 @@ target_include_directories(global_planner_adapter PRIVATE
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
+# Ensure runtime loader prefers in-tree adapter libs over system/ROS libs.
+# Without this, dlopen() may resolve libcostmap_adapter.so from /opt/ros/noetic/lib,
+# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
+set(_NAV_CORE_ADAPTER_RPATH
+    "${CMAKE_BINARY_DIR}/src/Navigations/Cores/nav_core_adapter"
+    "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
+    "${CMAKE_BINARY_DIR}/src/Libraries/tf3"
+    "${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
+    "${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
+)
+
+set_target_properties(costmap_adapter PROPERTIES
+  BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  LINK_FLAGS "-Wl,--disable-new-dtags"
+)
+set_target_properties(local_planner_adapter PROPERTIES
+  BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  LINK_FLAGS "-Wl,--disable-new-dtags"
+)
+set_target_properties(global_planner_adapter PROPERTIES
+  BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  LINK_FLAGS "-Wl,--disable-new-dtags"
+)
+
 # Cài đặt header files
 install(DIRECTORY include/nav_core_adapter 
         DESTINATION include

src/Navigations/Cores/nav_core_adapter/README.md

@@ -3,7 +3,7 @@ This package contains adapters for using `nav_core` plugins as `nav_core2` plugi
  * Converting between 2d and 3d datatypes.
  * Converting between returning false and throwing exceptions on failure.
 
-We also provide an adapter for using a `costmap_2d::Costmap2DROS` as a plugin for the `nav_core2::Costmap` interface.
+We also provide an adapter for using a `costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
 
 ## Adapter Classes
  * Global Planner Adapters
@@ -13,8 +13,8 @@ as a `nav_core2` plugin, like in `locomotor`.
  * Local Planner Adapter
    * `LocalPlannerAdapter` is used for employing a `nav_core2` local planner interface (such as `dwb_local_planner`) as a `nav_core` plugin, like in `move_base`. In addition to the standard adaptation steps listed above, the local planner adapter also uses the costmap to grab the global pose and subscribes to the odometry in order to get the current velocity.
    * There is no `LocalPlannerAdapter2`. The `nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `nav_core` counterpart. This information would be ignored by a `nav_core` planner, so no adapter is provided.
- * `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROS` object. It is not a perfect adaptation, because
-   * `Costmap2DROS` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though.
+ * `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROBOT` object. It is not a perfect adaptation, because
+   * `Costmap2DROBOT` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though.
    * `setInfo` is not implemented.
 
 ## Parameter Setup

src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h

@@ -48,14 +48,14 @@ class CostmapAdapter : public nav_core2::Costmap
 {
 public:
   /**
-   * @brief Deconstructor for possibly freeing the costmap_ros_ object
+   * @brief Deconstructor for possibly freeing the costmap_robot_ object
    */
   virtual ~CostmapAdapter();
 
   /**
-   * @brief Initialize from an existing Costmap2DROS object
-   * @param costmap_ros A Costmap2DROS object
-   * @param needs_destruction Whether to free the costmap_ros object when this class is destroyed
+   * @brief Initialize from an existing Costmap2DROBOT object
+   * @param costmap_robot A Costmap2DROBOT object
+   * @param needs_destruction Whether to free the costmap_robot object when this class is destroyed
    */
   void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
 

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

@@ -196,11 +196,10 @@ namespace nav_core_adapter
     costmap_2d::Costmap2DROBOT *costmap_robot_;
 
     boost::recursive_mutex configuration_mutex_;
-    // std::shared_ptr<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>> dsrv_;
 
     robot::NodeHandle private_nh_;
     robot::NodeHandle adapter_nh_;
-    // void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level);
+
     nav_core_adapter::NavCoreAdapterConfig last_config_;
     nav_core_adapter::NavCoreAdapterConfig default_config_;
     bool setup_;