fix move base

config/costmap_common_params.yaml

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

config/dock_global_params.yaml

@@ -0,0 +1,11 @@
+DockPlanner:
+  library_path: libdock_planner
+  MyGlobalPlanner:
+    cost_threshold: 200        # Ngưỡng chi phí vật cản (0-255)
+    safety_distance: 2         # Khoảng cách an toàn (cells)
+    use_dijkstra: false        # Sử dụng Dijkstra thay vì A*
+    
+  # File: config/costmap_params.yaml
+  global_costmap:
+    inflation_radius: 0.3      # Bán kính phình vật cản
+    cost_scaling_factor: 10.0  # Hệ số tỷ lệ chi phí

config/move_base_common_params.yaml

@@ -1,3 +1,25 @@
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+base_local_planner: LocalPlannerAdapter
+base_global_planner: CustomPlanner
+
+PNKXLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: CustomPlanner
+
+PNKXDockingLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXGoStraightLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXRotateLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
 
 ### replanning
 controller_frequency: 30.0   # run controller at 15.0 Hz

config/pnkx_local_planner_params.yaml

@@ -41,8 +41,8 @@ PNKXRotateLocalPlanner:
     
 LimitedAccelGenerator:
   library_path: libmkt_plugins_standard_traj_generator
-  max_vel_x: 1.0
+  max_vel_x: 1.2
-  min_vel_x: -1.0
+  min_vel_x: -1.2
 
   max_vel_y: 0.0  # diff drive robot
   min_vel_y: 0.0  # diff drive robot

examples/NavigationExample/NavigationAPI.cs

@@ -9,7 +9,7 @@ namespace NavigationExample
     /// </summary>
     public class NavigationAPI
     {
-        private const string DllName = "libnav_c_api.so"; // Linux
+        private const string DllName = "/usr/local/lib/libnav_c_api.so"; // Linux
         // For Windows: "nav_c_api.dll"
         // For macOS: "libnav_c_api.dylib"
 
@@ -152,6 +152,10 @@ namespace NavigationExample
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, string marker, PoseStamped goal);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_nodes_edges(NavigationHandle handle, string marker, Node[] nodes, Edge[] edges, PoseStamped goal);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);

examples/NavigationExample/NavigationExample.csproj

@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net10.0</TargetFramework>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
   <ItemGroup>

examples/NavigationExample/Program.cs

@@ -396,8 +396,8 @@ namespace NavigationExample
 
             PoseStamped goal = new PoseStamped();
             goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
-            goal.pose.position.x = 1.0;
+            goal.pose.position.x = 0.01;
-            goal.pose.position.y = 1.0;
+            goal.pose.position.y = 0.01;
             goal.pose.position.z = 0.0;
             goal.pose.orientation.x = 0.0;
             goal.pose.orientation.y = 0.0;
@@ -406,9 +406,9 @@ namespace NavigationExample
 
 
             // Console.WriteLine("Docking to docking_point");
-            // NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal);
+            NavigationAPI.navigation_dock_to(navHandle, "charger", goal);
 
-            NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);
+            // NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);
 
             // NavigationAPI.navigation_move_to_order(navHandle, order, goal);
             NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);

examples/run_example.sh

@@ -27,6 +27,7 @@ echo "Building C API library..."
 cd "$BUILD_DIR"
 cmake ..
 make 
+sudo make install
 echo "Library built successfully!"
 
 
@@ -83,9 +84,9 @@ fi
 # Luôn copy source code mới nhất (cập nhật file nếu đã có)
 cd "$EXAMPLE_DIR/NavigationExample"
 
-# Bước 3: Copy library
+# # Bước 3: Copy library
-echo "Copying library..."
+# echo "Copying library..."
-cp "$LIB_DIR/libnav_c_api.so" .
+# cp "$LIB_DIR/libnav_c_api.so" .
 
 # Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
 # Main build directory (contains libtf3.so, librobot_cpp.so, etc.)

src/APIs/c_api/include/nav_c_api.h

@@ -171,6 +171,20 @@ extern "C"
    */
   bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal);
 
+    /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param marker Marker name or ID (null-terminated string)
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+   bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
+
+
   /**
    * @brief Move straight toward the target position
    * @param handle Navigation handle

src/APIs/c_api/src/nav_c_api.cpp

@@ -83,6 +83,7 @@ extern "C" NavigationHandle navigation_create(void)
     // Using default constructor - initialization will be done via navigation_initialize()
     robot::PluginLoaderHelper loader;
     std::string path_file_so = loader.findLibraryPath("MoveBase");
+    robot::log_warning("%s", path_file_so.c_str());
     auto move_base_loader = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
         path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
     auto move_base_ptr = move_base_loader();
@@ -397,6 +398,44 @@ extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order or
   }
 }
 
+extern "C" bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal)
+{
+  if (!handle || !marker)
+  {
+    return false;
+  }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
+  try
+  {
+    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
+        std::shared_ptr<robot::move_base_core::BaseNavigation>(
+            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
+    if (!nav_ptr)
+      return false;
+    Order order;
+    order.nodes = const_cast<Node *>(nodes);
+    order.nodes_count = node_count;
+    order.edges = const_cast<Edge *>(edges);
+    order.edges_count = edge_count;
+    robot_protocol_msgs::Order cpp_order = convert2CppOrder(order);
+    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
+    return nav_ptr->dockTo(cpp_order, std::string(marker), cpp_goal);
+  }
+  catch (const std::exception &e)
+  {
+    return false;
+  }
+  catch (...)
+  {
+    printf("[%s:%d]\n Error: Failed to dock to nodes edges\n", __FILE__, __LINE__);
+    return false;
+  }
+}
+
 extern "C" bool navigation_move_straight_to(NavigationHandle handle, const double distance)
 {
   if (!handle)

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

@@ -602,7 +602,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 {
   // 1) Curvature from pure pursuit
   const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
-  const double L2_min = 0.05;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
+  const double L2_min = 0.01;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
   const double L2_safe = std::max(L2, L2_min);
   const double L = std::sqrt(L2_safe);
 
@@ -631,7 +631,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
     v_target = std::copysign(min_approach_linear_velocity, sign_x);
 
   // 5) Angular speed from curvature
-  double w_target = v_target * kappa;
+  double w_target = v_target * kappa + std::copysign(carrot_pose.pose.theta * dt, kappa);
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
   {
     if (trajectory.poses.size() >= 2) {
@@ -789,16 +789,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   //   (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
 
   bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
-#ifdef BUILD_WITH_ROS
+// #ifdef BUILD_WITH_ROS
-  if (result)
+  // if (result)
-    ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+  //   ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
 
   // else if(fabs(velocity.x) < min_speed_xy_)
   // {
   //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
   //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
   // }
-#endif
+// #endif
   return result;
 }
 

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

@@ -36,8 +36,6 @@ namespace two_points_planner
     if (!initialized_)
     {
       robot::NodeHandle nh_priv_("~/" + name);
-      robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());
-
       int lethal_obstacle;
       nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
       lethal_obstacle_ = (unsigned char)lethal_obstacle;
@@ -121,6 +119,7 @@ namespace two_points_planner
       robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
       return false;
     }
+    
     robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start);
     robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);
     robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", 
@@ -148,21 +147,21 @@ namespace two_points_planner
     }
 
     plan.clear();
-    plan.push_back(start);
+    // plan.push_back(start);
     
-    unsigned int mx_start, my_start;
+    // unsigned int mx_start, my_start;
-    unsigned int mx_end, my_end;
+    // unsigned int mx_end, my_end;
-    if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
+    // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
-      start.pose.position.y,
+    //   start.pose.position.y,
-      mx_start, my_start)
+    //   mx_start, my_start)
 
-      || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
+    //   || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
-        goal.pose.position.y,
+    //     goal.pose.position.y,
-        mx_end, my_end))
+    //     mx_end, my_end))
-    {
+    // {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
+    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
+    //   return false;
-    }
+    // }
     // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
     // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
     // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
@@ -175,8 +174,9 @@ namespace two_points_planner
     // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
     const double dx = goal.pose.position.x - start.pose.position.x;
     const double dy = goal.pose.position.y - start.pose.position.y;
-    double distance = std::sqrt(dx * dx + dy * dy);
+    const double distance = std::sqrt(dx * dx + dy * dy);
     double theta;
+
     if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
     {
       theta = std::atan2(dy, dx);
@@ -188,8 +188,12 @@ namespace two_points_planner
     }
     else 
     { 
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
+      robot_geometry_msgs::PoseStamped pose = start;
-      return false;
+      pose.pose.position.x += 0.01 * cos(theta);
+      pose.pose.position.y += 0.01 * sin(theta);
+      plan.push_back(pose);
+      plan.push_back(goal);
+      return true;
     }
 
     // Lấy độ phân giải của costmap

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

@@ -74,7 +74,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -83,7 +83,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_nav_name; 
@@ -99,14 +99,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
@@ -121,7 +121,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
       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());
@@ -129,7 +129,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
@@ -145,7 +145,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -153,7 +153,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     this->initializeOthers();
@@ -250,11 +250,16 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
   if(rotate_algorithm_) rotate_algorithm_->reset();
   ret_nav_ = ret_angle_ = false;
 
+  robot::log_info_at(__FILE__, __LINE__, "Debug");
+  parent_.printParams();
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+  // parent_.setParam(algorithm_nav_name, original_papams_);
   robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
-  parent_.setParam(algorithm_nav_name, original_papams_);
   nh_algorithm.setParam("allow_rotate", false);
+
+  robot::log_info_at(__FILE__, __LINE__, "Debug ở đây");
+  parent_.printParams();
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -393,7 +398,6 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
   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_)
@@ -401,9 +405,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
         traj = dkpl_.front()->docking_nav_->calculator(pose, velocity);
       }
     }
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
   }
   else
+  {
     traj = rotate_algorithm_->calculator(pose, velocity);
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
+  }
 
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
@@ -567,18 +579,51 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name)
-    : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false),
+    : initialized_(false),
-      is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
+      is_detected_(false),
+      is_goal_reached_(false),
+      following_(false),
+      allow_rotate_(false),
+      xy_goal_tolerance_(0.05),
+      yaw_goal_tolerance_(0.05),
+      min_lookahead_dist_(0.4),
+      max_lookahead_dist_(1.0),
+      lookahead_time_(1.5),
+      angle_threshold_(0.4),
+      name_(name),
+      docking_planner_name_(),
+      docking_nav_name_(),
+      docking_planner_(nullptr),
+      docking_nav_(nullptr),
+      linear_(),
+      angular_(),
+      nh_(),
+      nh_priv_(),
+      tf_(nullptr),
+      costmap_robot_(nullptr),
+      traj_generator_(),
+      docking_planner_loader_(),
+      docking_nav_loader_(),
+      detected_timeout_wt_(),
+      delayed_wt_(),
+      delayed_(false),
+      detected_timeout_(false),
+      robot_base_frame_(),
+      maker_goal_frame_()
 {
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
 {
-  if (docking_planner_)
+  detected_timeout_wt_.stop();
+  delayed_wt_.stop();
+  if (docking_planner_ != nullptr) {
     docking_planner_.reset();
-  if (docking_nav_)
+  }
+  if (docking_nav_ != nullptr) {
     docking_nav_.reset();
   }
+}
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                                                                              score_algorithm::TrajectoryGenerator::Ptr traj_generator)
@@ -593,14 +638,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(docking_planner_name_);
       docking_planner_loader_ = boost::dll::import_alias<robot_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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create global planner");
       }
       docking_planner_->initialize(name_, costmap_robot_);
       robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str());
@@ -616,14 +662,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(docking_nav_name_);
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create docking nav");
       }
       robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_);
       docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);

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

@@ -80,7 +80,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_nav_name = std::string("pnkx_local_planner::GoStraight");
@@ -104,14 +104,14 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string goal_checker_name;
@@ -126,7 +126,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
       goal_checker_->initialize(nh_goal_checker);
@@ -134,7 +134,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str());

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

@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -97,7 +97,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s 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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
     
     std::string algorithm_nav_name; 
@@ -107,20 +107,21 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str());
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
@@ -135,7 +136,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
       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());
@@ -143,7 +144,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s 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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
@@ -159,7 +160,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -167,7 +168,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     this->initializeOthers();

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

@@ -64,7 +64,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -72,7 +72,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
       nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
@@ -96,7 +96,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string goal_checker_name;
@@ -111,7 +111,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
       goal_checker_->initialize(nh_goal_checker);
@@ -119,7 +119,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
     
     robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());

src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h

@@ -89,7 +89,7 @@ public:
 
 private:
   /**
-   * @brief Resolve library path (handle relative paths, search in search_paths)
+   * @brief Resolve library path (absolute, then PNKX_NAV_CORE_LIBRARY_PATH, search_paths_, cwd)
    * @param library_path Path from config (may be relative or absolute)
    * @return Resolved absolute path, or empty if not found
    */

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -651,7 +651,8 @@ namespace robot
           std::string key = it->first.as<std::string>();
           const YAML::Node &value = it->second;
           std::string full_key = prefix.empty() ? key : prefix + "/" + key;
-
+          if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos)
+            continue;
           if (value.IsMap())
           {
             std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl;

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

@@ -66,6 +66,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
   }
   // Try to read from NodeHandle
   std::string library_path;
+  robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str());
   if (nh_.hasParam(param_path)) {
     nh_.getParam(param_path, library_path, std::string(""));
     if (!library_path.empty()) {
@@ -97,7 +98,6 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
       }
     }
   }
-
   // Try LD_LIBRARY_PATH as fallback
   const char* ld_path = std::getenv("LD_LIBRARY_PATH");
   if (ld_path) {
@@ -198,16 +198,21 @@ std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_pa
     return "";
   }
 
-  // If relative path, search in search_paths (build directory is already added)
+  // If relative path, search under PNKX_NAV_CORE_LIBRARY_PATH (':'-separated, like LD_LIBRARY_PATH)
-  std::string build_dir = getBuildDirectory();
+  const char* nav_lib_path_env = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
-  if (!build_dir.empty()) {
+  if (nav_lib_path_env) {
-    // First try in build directory
-    // Add .so extension if not present
     std::string lib_path_with_ext = library_path;
     if (lib_path_with_ext.find(".so") == std::string::npos) {
       lib_path_with_ext += ".so";
     }
-    std::filesystem::path full_path = std::filesystem::path(build_dir) / lib_path_with_ext;
+    std::string nav_lib_paths(nav_lib_path_env);
+    std::stringstream ss(nav_lib_paths);
+    std::string base_dir;
+    while (std::getline(ss, base_dir, ':')) {
+      if (base_dir.empty()) {
+        continue;
+      }
+      std::filesystem::path full_path = std::filesystem::path(base_dir) / lib_path_with_ext;
       if (std::filesystem::exists(full_path)) {
         try {
           return std::filesystem::canonical(full_path).string();
@@ -216,6 +221,7 @@ std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_pa
         }
       }
     }
+  }
 
   // Try in search_paths
   for (const auto& base_path : search_paths_) {
@@ -338,7 +344,7 @@ std::string PluginLoaderHelper::getBuildDirectory()
 std::string PluginLoaderHelper::getWorkspacePath()
 {
   // Method 1: Từ environment variable PNKX_NAV_CORE_DIR
-  const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
+  const char* workspace_path = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
   if (workspace_path && std::filesystem::exists(workspace_path)) {
     return std::string(workspace_path);
   }

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

@@ -41,6 +41,7 @@
 #include <robot_geometry_msgs/Twist.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <tf3/buffer_core.h>
+#include <robot_nav_msgs/Odometry.h>
 #include <memory>
 
 namespace robot_nav_core
@@ -148,6 +149,12 @@ namespace robot_nav_core
      */
     virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
 
+    /**
+     * @brief Set the odometry of the robot
+     * @param odom The odometry of the robot
+     */
+    virtual void setOdom(robot_nav_msgs::Odometry *odom) { odom_ = odom; }
+
     /**
      * @brief  Virtual destructor for the interface
      */
@@ -155,6 +162,11 @@ namespace robot_nav_core
 
   protected:
     BaseLocalPlanner() {}
+
+    /**
+     * @brief The odometry of the robot
+     */
+     robot_nav_msgs::Odometry *odom_;
   };
 } // namespace robot_nav_core
 

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

@@ -189,11 +189,6 @@ namespace robot_nav_core_adapter
     robot_nav_2d_msgs::Pose2DStamped last_goal_;
     bool has_active_goal_;
 
-    /**
-     * @brief The odometry of the robot
-     */
-    robot_nav_msgs::Odometry odom_;
-
     // Plugin handling
     boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
     robot_nav_core2::LocalPlanner::Ptr planner_;

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

@@ -291,7 +291,7 @@ namespace robot_nav_core_adapter
 
   robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
   {
-    return odom_.twist.twist;
+    return odom_->twist.twist;
   }
 
   bool LocalPlannerAdapter::islock()
@@ -356,7 +356,7 @@ namespace robot_nav_core_adapter
     if (!getRobotPose(pose2d))
       return false;
 
-    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist);
+    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist);
     bool ret = planner_->isGoalReached(pose2d, velocity);
     if (ret)
     {

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

@@ -31,7 +31,9 @@
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_nav_2d_utils/conversions.h>
+#ifndef BUILD_WITH_ROS
   #include <laser_filter/laser_filter.h>
+#endif
 
 move_base::MoveBase::MoveBase()
   : initialized_(false),
@@ -269,6 +271,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(global_planner);
+      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, global_planner, boost::dll::load_mode::append_decorations);
 
@@ -312,6 +315,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(local_planner);
+      robot::log_info("[%s:%d]\n INFO: local_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       controller_loader_ =
           boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
               path_file_so, local_planner, boost::dll::load_mode::append_decorations);
@@ -322,6 +326,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
         throw std::runtime_error("Failed to load local planner " + local_planner);
       }
       tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
+      tc_->setOdom(&odometry_);
     }
     catch (const std::exception &ex)
     {
@@ -398,7 +403,7 @@ void move_base::MoveBase::swapPlanner(std::string base_global_planner)
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(base_global_planner);
-
+      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
       auto new_planner = new_loader();
@@ -500,6 +505,7 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin
 void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
 {
   auto it = laser_scans_.find(laser_scan_name);
+#ifndef BUILD_WITH_ROS
   laser_filter::LaserScanSOR sor;
   sor.setMeanK(10);              // xét 10 điểm láng giềng gần nhất
   sor.setStddevMulThresh(1.0);   // ngưỡng = mean + 1.0 * stddev
@@ -512,6 +518,16 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   {
     it->second = laser_scan_filter;
   }
+#else
+  if (it == laser_scans_.end())
+  {
+    laser_scans_[laser_scan_name] = laser_scan;
+  }
+  else
+  {
+    it->second = laser_scan;
+  }
+#endif
   // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
   // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
   // robot::log_info("angle_min: %f", laser_scan.angle_min);
@@ -534,8 +550,13 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   // }
   // robot::log_error("intensities: %s", intensities_str.str().c_str());
 
+#ifndef BUILD_WITH_ROS
   updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
   updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+#else
+  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+#endif
 }
 
 robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)
@@ -811,9 +832,19 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
                                   double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   if (setup_)
+  {
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
     {
       swapPlanner(default_config_.base_global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -962,9 +993,19 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
                                  double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   if (setup_)
+  {
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
     {
       swapPlanner(default_config_.base_global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1119,9 +1160,19 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
   }
 
   if (setup_)
+  {
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
     {
       swapPlanner(default_config_.base_global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1195,6 +1246,47 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
   }
   if (cancel_ctr_)
     cancel_ctr_ = false;
+  // Check if action server exists
+  if (!as_)
+  {
+    robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
+    lock.unlock();
+    return false;
+  }
+
+  try
+  {
+    robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
+    action_goal->header.stamp = robot::Time::now();
+    action_goal->goal.target_pose = goal;
+    
+    // Generate unique goal ID using timestamp
+    robot::Time now = robot::Time::now();
+    action_goal->goal_id.stamp = now;
+    std::ostringstream goal_id_stream;
+    goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
+    action_goal->goal_id.id = goal_id_stream.str();
+    
+    robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
+    robot::log_info("[MoveBase::moveTo] Goal stamp: %ld.%09ld", 
+                    action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
+    
+    // Clear Order message since this is a non-Order moveTo call
+    {
+      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
+      planner_order_.reset();
+    }
+    
+    robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
+    as_->processGoal(action_goal);
+    robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
+  }
+  catch (const std::exception &e)
+  {
+    lock.unlock();
+    robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what());
+    return false;
+  }
 
   lock.unlock();
   return true;
@@ -1232,9 +1324,19 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
   }
 
   if (setup_)
+  {
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
     {
       swapPlanner(default_config_.base_global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1342,8 +1444,18 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, go_straight_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
       swapPlanner(global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1465,8 +1577,18 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, rotate_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
       swapPlanner(global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1662,32 +1784,6 @@ void move_base::MoveBase::cancel()
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   cancel_ctr_ = true;
   robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true");
-  
-  if (as_)
-  {
-    // Get current goal ID from action server to cancel specific goal
-    // If we want to cancel all goals, use empty string ""
-    robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
-    
-    // Use empty string to cancel current goal (processCancel accepts empty string to cancel all)
-    msg->id = "";  // Empty string cancels current goal
-    msg->stamp = robot::Time::now();
-    
-    robot::log_info("[MoveBase::cancel] Sending cancel request to action server");
-    robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld", 
-                    msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec);
-    
-    // Convert to ConstPtr for processCancel
-    robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
-    as_->processCancel(cancel_msg);
-    robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
-  }
-  else
-  {
-    robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
-  }
-  
-  robot::log_info("[MoveBase::cancel] ===== EXIT =====");
 }
 
 bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
@@ -1911,6 +2007,7 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
         std::string behavior_name = behavior["name"].as<std::string>();
         robot::PluginLoaderHelper loader;
         std::string path_file_so = loader.findLibraryPath(behavior_type);
+        robot::log_info("Loading recovery behavior '%s' of type '%s' from '%s'", behavior_name.c_str(), behavior_type.c_str(), path_file_so.c_str());
         // Load the factory function from the shared library
         recovery_loaders_[behavior_name] = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
             path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
@@ -2262,8 +2359,9 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
       robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
     }
   }
-
+  robot::log_warning("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
   robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
+  robot::log_warning("Received goalToGlobalFrame: x=%.2f, y=%.2f", goal.pose.position.x, goal.pose.position.y);
   publishZeroVelocity();
   // we have a goal so start the planner
   boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2483,7 +2581,6 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
     robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
     return false;
   }
-
   tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
 
   // next, we need to check if the length of the quaternion is close to zero
@@ -2645,6 +2742,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       recovery_index_ = 0;
   }
 
+
   // Cancle executeCycle
   if (cancel_ctr_ && tc_)
   {
@@ -2653,13 +2751,25 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     tc_->setTwistLinear(linear);
     try
     {
-      if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
+      double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
+      if (actual_linear_velocity <= min_approach_linear_velocity_)
       {
         robot::log_info("MoveTo is canceled.");
         resetState();
+        cancel_ctr_ = false;
         // if(default_config_.base_global_planner != last_config_.base_global_planner)
+        robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+        if(nh_planner.hasParam("base_global_planner"))
+        {
+          std::string base_global_planner;
+          nh_planner.getParam("base_global_planner", base_global_planner);
+          swapPlanner(base_global_planner);
+        }
+        else
+        {
           swapPlanner(default_config_.base_global_planner);
-
+        }
+        cancel_ctr_ = false;
         // disable the planner thread
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         runPlanner_ = false;
@@ -2670,7 +2780,21 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
           nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
         }
         // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
-        cancel_ctr_ = false;
+
+        if (as_)
+        {
+          robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
+          msg->id = "";  // Empty string cancels current goal
+          msg->stamp = robot::Time::now();
+          // Convert to ConstPtr for processCancel
+          robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
+          as_->processCancel(cancel_msg);
+          robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
+        }
+        else
+        {
+          robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
+        }
         return true;
       }
     }
@@ -2679,7 +2803,17 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       robot::log_info("MoveTo is canceled.");
       resetState();
       // if(default_config_.base_global_planner != last_config_.base_global_planner)
+      robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+      if(nh_planner.hasParam("base_global_planner"))
+      {
+        std::string base_global_planner;
+        nh_planner.getParam("base_global_planner", base_global_planner);
+        swapPlanner(base_global_planner);
+      }
+      else
+      {
         swapPlanner(default_config_.base_global_planner);
+      }
 
       // disable the planner thread
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2691,6 +2825,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
         nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
       }
       // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
+      if (as_)
+      {
+        robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
+        msg->id = "";  // Empty string cancels current goal
+        msg->stamp = robot::Time::now();
+        // Convert to ConstPtr for processCancel
+        robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
+        as_->processCancel(cancel_msg);
+        robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
+      }
+      else
+      {
+        robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
+      }
       cancel_ctr_ = false;
       return true;
     }
@@ -2711,7 +2859,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
   // if we're controlling, we'll attempt to find valid velocity commands
   case move_base::CONTROLLING:
-    // robot::log_debug("In controlling state.");
 
     // check to see if we've reached our goal
     try
@@ -2720,7 +2867,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       {
         robot::log_debug("Goal reached!");
         resetState();
-        swapPlanner(default_config_.base_global_planner);
+        // swapPlanner(default_config_.base_global_planner);
         // disable the planner thread
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         runPlanner_ = false;
@@ -2885,7 +3032,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
         tc_->setTwistLinear(linear);
         try
         {
-          if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
+          double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
+          if (actual_linear_velocity <= min_approach_linear_velocity_)
           {
             paused_ = true;
           }
@@ -2925,7 +3073,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     {
       robot::log_debug("All recovery behaviors have failed, locking the planner and disabling it.");
       resetState();
-      swapPlanner(default_config_.base_global_planner);
+      // swapPlanner(default_config_.base_global_planner);
 
       // disable the planner thread
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2989,16 +3137,49 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
 robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
 {
-  std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
-  robot_geometry_msgs::PoseStamped goal_pose, global_pose;
-  goal_pose = goal_pose_msg;
 
-  goal_pose.header.stamp = robot::Time(); // latest available
+  if (!tf_)
+  {
+    return goal_pose_msg;
+  }
+
+  std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
+  // robot_geometry_msgs::PoseStamped goal_pose, global_pose;
+  // goal_pose = goal_pose_msg;
+
+  // goal_pose.header.stamp = robot::Time(); // latest available
+  // try
+  // {
+  //   tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
+  //   tf3::doTransform(goal_pose, global_pose, transform);
+  // }
+  robot_geometry_msgs::PoseStamped global_pose;
+  tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
+  robot_geometry_msgs::PoseStamped goal_pose;
+  tf3::toMsg(tf3::Transform::getIdentity(), goal_pose.pose);
+  goal_pose = goal_pose_msg;
+  robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
+  tf3::Time tf3_current_time = data_convert::convertTime(current_time);
+  tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
+  
   try
   {
-    tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
+    // use current time if possible (makes sure it's not in the future)
+    std::string error_msg;
+    if (tf_->canTransform(global_frame, goal_pose.header.frame_id, tf3_current_time, &error_msg))
+    {
+      // Transform is available at current time
+      tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_current_time);
       tf3::doTransform(goal_pose, global_pose, transform);
     }
+    // use the latest otherwise (tf3::Time() means latest available)
+    else
+    {
+      // Try to get latest transform
+      tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_zero_time);
+      tf3::doTransform(goal_pose, global_pose, transform);
+    }
+  }
   catch (tf3::LookupException &ex)
   {
     robot::log_error("Move Base No Transform available Error looking up robot pose: %s", ex.what());