update find path file plugin .so

.gitignore

@@ -421,3 +421,5 @@ FodyWeavers.xsd
 build
 install
 devel
+
+obstacle

.gitmodules

@@ -28,3 +28,6 @@
 [submodule "src/Libraries/xmlrpcpp"]
 	path = src/Libraries/xmlrpcpp
 	url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git
+[submodule "src/Libraries/laser_filter"]
+	path = src/Libraries/laser_filter
+	url = https://git.pnkr.asia/DuongTD/laser_filter.git

CMAKE_BUILD_README.md

@@ -51,6 +51,7 @@ sudo apt-get install -y \
     libyaml-cpp-dev \
     libpcl-dev \
     libgoogle-glog-dev
+sudo apt install liborocos-kdl-dev
 
 # Optional: Cài đặt Google Test (nếu muốn build tests)
 sudo apt-get install -y libgtest-dev

CMakeLists.txt

@@ -74,6 +74,10 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
+if (NOT TARGET laser_filter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter) 
+endif()
+
 if (NOT TARGET robot_nav_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
 endif()
@@ -134,6 +138,22 @@ if (NOT TARGET pnkx_local_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
 endif()
 
+if (NOT TARGET robot_angles)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/angles)
+endif()
+
+if (NOT TARGET grid_map_core)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/grid_map_core)
+endif()
+
+if (NOT TARGET robot_base_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/base_local_planner)
+endif()
+
+if (NOT TARGET hybrid_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/hybrid_local_planner)
+endif()
+
 if (NOT TARGET robot_actionlib_msgs)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
 endif()

config/config/obstacle_layer_params.yaml

@@ -1,4 +1,5 @@
 obstacle_layer:
+  enabled: true
   track_unknown_space: true
   transform_tolerance: 0.2
   topic: "map"

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
@@ -26,21 +19,37 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-  f_scan_marking:
-    topic: /f_scan
+  observation_sources:  l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing
+  l_scan_marking:
+    topic: /l_scan
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
-  f_scan_clearing:
-    topic: /f_scan
+  l_scan_clearing:
+    topic: /l_scan
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_marking:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_clearing:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_marking:
@@ -48,7 +57,7 @@ obstacles:
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_clearing:
@@ -56,8 +65,9 @@ obstacles:
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   
 
+

config/costmap_global_params.yaml

@@ -4,7 +4,7 @@ global_costmap:
   global_frame: map
   update_frequency: 1.0
   publish_frequency: 1.0
-  raytrace_range: 2.0
+  raytrace_range: 3.5
   resolution: 0.05
   z_resolution: 0.2
   rolling_window: false

config/costmap_local_params.yaml

@@ -5,7 +5,7 @@ local_costmap:
   update_frequency: 6.0
   publish_frequency: 6.0
   rolling_window: true
-  raytrace_range: 2.0
+  raytrace_range: 3.5
   resolution: 0.05
   z_resolution: 0.15
   z_voxels: 8

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/hybrid_local_planner_params.yaml

@@ -0,0 +1,59 @@
+LocalPlannerAdapter:
+  library_path: liblocal_planner_adapter
+  yaw_goal_tolerance: 0.017  
+  xy_goal_tolerance: 0.03 
+  min_approach_linear_velocity: 0.06
+
+HybridLocalPlanner:
+# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
+# HybridLocalPlanner:
+    library_path: libhybrid_local_planner
+    odom_topic: odom
+    # Trajectory
+    max_global_plan_lookahead_dist: 4.0
+    global_plan_viapoint_sep: 0.5
+    global_plan_prune_distance: 0.0
+    global_plan_goal_sep: 0.05
+
+    # Robot
+
+    robot_max_v_ac: 0.4
+    robot_max_w_ac: 0.4
+    robot_max_v_pt: 1.0
+    robot_max_w_pt: 0.4
+    robot_max_v_backwards_pt: -0.2
+    acc_lim_x: 1.0
+    acc_lim_theta: 2.0
+    turn_around_priority: True
+    stop_dist: 0.5
+    dec_dist: 1.0
+
+
+    # GoalTolerance
+
+    xy_goal_tolerance: 0.1
+    yaw_goal_tolerance: 0.07
+
+    # Optimization
+
+    # PP Parameters
+    w_vel: 0.8
+    w_omega: 1.0
+    # DWA Parameters
+    enable_backward_motion: false
+    w_targetheading_ac: 1.7
+    w_velocity_ac: 0.2
+    w_clearance_ac: 0.2
+    w_pathDistance_ac: 0.05
+    w_smoothness_ac: 0.3
+    w_targetheading_pt: 0.2
+    w_velocity_pt: 0.8
+    w_clearance_pt: 0.1
+    w_pathDistance_pt: 2.1
+    w_smoothness_pt: 0.3
+    time_horizon: 3.0
+    velocity_resolution: 0.015
+
+    segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment
+    calibration_factor: 1.5 # Hệ số hiệu chuẩn
+    use_obstacle_avoidance: true # Bật tắt tránh vật cản

config/maker_sources.yaml

@@ -37,7 +37,7 @@ charger:
     - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
 
   charger:
-    maker_goal_frame: charger_goal
+    maker_goal_frame: charger
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được
     timeout: 60

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: 0.2
-  min_vel_x: -0.2
+  max_vel_x: 1.2
+  min_vel_x: -1.2
 
   max_vel_y: 0.0  # diff drive robot
   min_vel_y: 0.0  # diff drive robot
@@ -50,7 +50,7 @@ LimitedAccelGenerator:
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
   min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
-  max_vel_theta: 0.7   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
   acc_lim_x: 1.5
@@ -79,6 +79,14 @@ MKTAlgorithmDiffPredictiveTrajectory:
   index_samples: 60
   follow_step_path: true
 
+  # Kalman filter tuning (filters v and w commands)
+  kf_q_v: 0.25
+  kf_q_w: 0.8
+  kf_r_v: 0.05
+  kf_r_w: 0.08
+  kf_p0: 0.5
+  kf_filter_angular: false
+
   # Lookahead
   use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
   # only when false:

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"
 
@@ -140,6 +140,10 @@ namespace NavigationExample
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to_nodes_edges(NavigationHandle handle, IntPtr nodes, UIntPtr node_count, IntPtr edges, UIntPtr edge_count, PoseStamped goal);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal);
@@ -148,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,12 +1,11 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net10.0</TargetFramework>
-    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
+    <TargetFramework>net6.0</TargetFramework>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
   <ItemGroup>
-    <PackageReference Include="SixLabors.ImageSharp" Version="3.1.5" />
+    <PackageReference Include="SixLabors.ImageSharp" Version="3.1.7" />
   </ItemGroup>
   <ItemGroup>
     <None Include="libnav_c_api.so">

examples/NavigationExample/Program.cs

@@ -396,25 +396,29 @@ namespace NavigationExample
 
             PoseStamped goal = new PoseStamped();
             goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
-            goal.pose.position.x = 1.0;
-            goal.pose.position.y = 1.0;
+            goal.pose.position.x = 0.01;
+            goal.pose.position.y = 0.01;
             goal.pose.position.z = 0.0;
             goal.pose.orientation.x = 0.0;
             goal.pose.orientation.y = 0.0;
             goal.pose.orientation.z = 0.0;
             goal.pose.orientation.w = 1.0;
 
-            NavigationAPI.navigation_dock_to_order(navHandle, order, "docking_point", goal);
 
-            // NavigationAPI.navigation_move_to_order(navHandle, order, goal);`
+            // Console.WriteLine("Docking to docking_point");
+            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_order(navHandle, order, goal);
             NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
             NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2);
 
             // NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
 
-            // while (true) 
-            // {
-            //     System.Threading.Thread.Sleep(100);
+            while (true) 
+            {
+                System.Threading.Thread.Sleep(100);
             //     NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
             //     if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
             //     {
@@ -474,7 +478,7 @@ namespace NavigationExample
             //             Console.WriteLine("Local Costmap is not updated");
             //         }
             //     }
-            // }
+            }
             // Cleanup (destroy nav first, then tf3 buffer)
 
             NavigationAPI.navigation_destroy(navHandle);
@@ -491,4 +495,3 @@ namespace NavigationExample
         }
     }
 }
-

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
-echo "Copying library..."
-cp "$LIB_DIR/libnav_c_api.so" .
+# # Bước 3: Copy library
+# echo "Copying library..."
+# 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/convertor.h

@@ -187,6 +187,13 @@ robot_protocol_msgs::Order convert2CppOrder(const Order& order);
  */
 Order convert2COrder(const robot_protocol_msgs::Order& cpp_order);
 
+/**
+ * @brief Check if a quaternion is valid
+ * @param q Quaternion
+ * @return True if valid, false otherwise
+ */
+bool isQuaternionValid(const Quaternion& q);
+
 /**
  * @brief Free dynamic memory held by an Order returned from convert2COrder
  * @param order Order to free (pointers and counts are zeroed)

src/APIs/c_api/include/nav_c_api.h

@@ -141,6 +141,18 @@ extern "C"
    */
   bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal);
 
+  /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @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_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
+
   /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle
@@ -159,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/convertor.cpp

@@ -796,3 +796,37 @@ extern "C" void order_free(Order* order)
   }
   order->edges_count = 0;
 }
+
+bool isQuaternionValid(const Quaternion& q)
+{
+  // first we need to check if the quaternion has nan's or infs
+  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
+  {
+    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
+  if (tf_q.length2() < 1e-6)
+  {
+    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
+    return false;
+  }
+
+  // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
+  tf_q.normalize();
+
+  tf3::Vector3 up(0, 0, 1);
+
+  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
+
+  if (fabs(dot - 1) > 1e-3)
+  {
+    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
+    return false;
+  }
+
+  return true;
+}

src/APIs/c_api/src/nav_c_api.cpp

@@ -171,6 +171,7 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
 
   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 *) {});
@@ -181,10 +182,12 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
     footprint.reserve(point_count);
     for (size_t i = 0; i < point_count; ++i)
     {
+      
       robot_geometry_msgs::Point pt;
       pt.x = points[i].x;
       pt.y = points[i].y;
       pt.z = points[i].z;
+      printf("footprint x: %f, y: %f\n", pt.x, pt.y);
       footprint.push_back(pt);
     }
     nav_ptr->setRobotFootprint(footprint);
@@ -237,7 +240,11 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped go
   {
     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 =
@@ -261,6 +268,11 @@ extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order or
   {
     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 =
@@ -284,12 +296,56 @@ extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order or
   }
 }
 
+extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal)
+{
+  if (!handle)
+  {
+    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;
+    robot::log_error("navigation_move_to_nodes_edges goal %f %f", goal.pose.position.x, goal.pose.position.y);
+    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->moveTo(cpp_order, cpp_goal);
+  }
+  catch (const std::exception &e)
+  {
+    return false;
+  }
+  catch (...)
+  {
+    printf("[%s:%d]\n Error: Failed to move to order\n", __FILE__, __LINE__);
+    return false;
+  }
+}
 extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal)
 {
   if (!handle || !marker)
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
 
   try
   {
@@ -310,10 +366,15 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker,
 
 extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal) 
 {
-  if (!handle)
+  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 =
@@ -336,13 +397,50 @@ 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)
   {
     return false;
   }
-
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -374,7 +472,11 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped
   {
     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 =

src/Algorithms/Libraries/kalman/src/kalman.cpp

@@ -55,11 +55,34 @@ void KalmanFilter::update(const Eigen::VectorXd& y) {
   if(!initialized)
     throw std::runtime_error("Filter is not initialized!");
 
+  if (y.size() != m)
+    throw std::runtime_error("KalmanFilter::update(): measurement vector has wrong size");
+
+  if (A.rows() != n || A.cols() != n || C.rows() != m || C.cols() != n ||
+      Q.rows() != n || Q.cols() != n || R.rows() != m || R.cols() != m ||
+      P.rows() != n || P.cols() != n)
+    throw std::runtime_error("KalmanFilter::update(): matrix dimensions are inconsistent");
+
   x_hat_new = A * x_hat;
   P = A*P*A.transpose() + Q;
-  K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
+
+  const Eigen::MatrixXd S = C * P * C.transpose() + R;
+  Eigen::LDLT<Eigen::MatrixXd> ldlt(S);
+  if (ldlt.info() != Eigen::Success)
+    throw std::runtime_error("KalmanFilter::update(): failed to decompose innovation covariance");
+
+  // K = P*C' * inv(S)  -> solve(S * X = (P*C')^T)
+  const Eigen::MatrixXd PCt = P * C.transpose();
+  const Eigen::MatrixXd KT = ldlt.solve(PCt.transpose());
+  if (ldlt.info() != Eigen::Success)
+    throw std::runtime_error("KalmanFilter::update(): failed to solve for Kalman gain");
+  K = KT.transpose();
+
   x_hat_new += K * (y - C*x_hat_new);
-  P = (I - K*C)*P;
+
+  // Joseph form: keeps P symmetric / PSD under numeric errors
+  const Eigen::MatrixXd IKC = I - K * C;
+  P = IKC * P * IKC.transpose() + K * R * K.transpose();
   x_hat = x_hat_new;
 
   t += dt;

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

@@ -394,12 +394,7 @@ namespace mkt_algorithm
       double near_goal_heading_last_error_;
       bool near_goal_heading_was_active_;
       
-      // Regulated linear velocity scaling
-      bool use_regulated_linear_velocity_scaling_;
-
       double min_approach_linear_velocity_;
-      double regulated_linear_scaling_min_radius_;
-      double regulated_linear_scaling_min_speed_;
 
       bool use_cost_regulated_linear_velocity_scaling_;
       double inflation_cost_scaling_factor_;
@@ -417,6 +412,14 @@ namespace mkt_algorithm
       Eigen::MatrixXd Q;
       Eigen::MatrixXd R;
       Eigen::MatrixXd P;
+
+      // Kalman filter tuning (for v and w filtering)
+      double kf_q_v_;
+      double kf_q_w_;
+      double kf_r_v_;
+      double kf_r_w_;
+      double kf_p0_;
+      bool kf_filter_angular_;
 #ifdef BUILD_WITH_ROS
       ros::Publisher lookahead_point_pub_;
 #endif

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

@@ -28,16 +28,19 @@ mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
     near_goal_heading_integral_(0.0),
     near_goal_heading_last_error_(0.0),
     near_goal_heading_was_active_(false),
-    use_regulated_linear_velocity_scaling_(false),
     min_approach_linear_velocity_(0.0),
-    regulated_linear_scaling_min_radius_(0.0),
-    regulated_linear_scaling_min_speed_(0.0),
     use_cost_regulated_linear_velocity_scaling_(false),
     inflation_cost_scaling_factor_(0.0),
     cost_scaling_dist_(0.0),
     cost_scaling_gain_(0.0),
     control_duration_(0.0),
     kf_(nullptr),
+    kf_q_v_(0.25),
+    kf_q_w_(0.8),
+    kf_r_v_(0.05),
+    kf_r_w_(0.08),
+    kf_p0_(0.5),
+    kf_filter_angular_(false),
     m_(0),
     n_(0)
 {
@@ -86,16 +89,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     // kalman
     last_actuator_update_ = robot::Time::now();
-    n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
-    m_ = 2; // measurements: x, y, theta
+    // State: [v, a, j, w, alpha, jw] where:
+    //  - v: linear velocity, a: linear accel, j: linear jerk
+    //  - w: angular velocity, alpha: angular accel, jw: angular jerk
+    // Measurement: [v, w]
+    n_ = 6;
+    m_ = 2;
     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_);
+    R = Eigen::MatrixXd::Zero(m_, m_);
+    P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_);
 
     for (int i = 0; i < n_; i += 3)
     {
@@ -106,15 +113,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     C(0, 0) = 1;
     C(1, 3) = 1;
-    Q(2, 2) = 0.1;
-    Q(5, 5) = 0.6;
+    Q(2, 2) = std::max(1e-12, kf_q_v_);
+    Q(5, 5) = std::max(1e-12, kf_q_w_);
 
-    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;
+    R(0, 0) = std::max(1e-12, kf_r_v_);
+    R(1, 1) = std::max(1e-12, kf_r_w_);
 
     kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
     Eigen::VectorXd x0(n_);
@@ -169,11 +172,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   if (trans_stopped_velocity_ > min_approach_linear_velocity_)
     trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01;
 
-  // Regulated linear velocity scaling
-  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
-  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
-  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
-
   // Inflation cost scaling (Limit velocity by proximity to obstacles)
   nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
   nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
@@ -185,6 +183,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     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;
   }
+
+  // Kalman filter tuning (filtering v and w commands)
+  nh_priv_.param<double>("kf_q_v", kf_q_v_, kf_q_v_);
+  nh_priv_.param<double>("kf_q_w", kf_q_w_, kf_q_w_);
+  nh_priv_.param<double>("kf_r_v", kf_r_v_, kf_r_v_);
+  nh_priv_.param<double>("kf_r_w", kf_r_w_, kf_r_w_);
+  nh_priv_.param<double>("kf_p0", kf_p0_, kf_p0_);
+  nh_priv_.param<bool>("kf_filter_angular", kf_filter_angular_, kf_filter_angular_);
+
   double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
   control_duration_ = 1.0 / control_frequency;
 
@@ -205,6 +212,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("min_speed_xy", min_speed_xy_, 0.0);
+    if(fabs(min_speed_xy_) > sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+    if(fabs(min_speed_xy_) > sqrt(min_vel_x_ * min_vel_x_ + min_vel_y_ * min_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+
     traj_.get()->getNodeHandle().param<double>("max_speed_xy", max_speed_xy_, 0.0);
   }
 }
@@ -545,7 +561,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       drive_cmd);
     }
     applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
-
     if (this->nav_stop_)
     {
       if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
@@ -558,27 +573,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       result.velocity = drive_cmd;
       return result;
     }
-
-    // Eigen::VectorXd y(2);
-    // y << drive_cmd.x, drive_cmd.theta;
-
-    // // Cập nhật lại A nếu dt thay đổi
-    // for (int i = 0; i < n_; ++i)
-    //   for (int j = 0; j < n_; ++j)
-    //     A(i, j) = (i == j ? 1.0 : 0.0);
-    // for (int i = 0; i < n_; i += 3)
-    // {
-    //   A(i, i + 1) = dt;
-    //   A(i, i + 2) = 0.5 * dt * dt;
-    //   A(i + 1, i + 2) = dt;
-    // }
-    // kf_->update(y, dt, A);
-    // double v_min = min_approach_linear_velocity_;
-    // drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
-    // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
   }
-
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
   for (const auto &pose_stamped : transformed_plan.poses)
@@ -607,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);
 
@@ -618,7 +613,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   const double L_min = 0.1;  // m, chỉnh theo nhu cầu
   double scale_close = std::clamp(L / L_min, 0.0, 1.0);
   v_target *= scale_close;
-
   const double y_abs = std::fabs(carrot_pose.pose.y);
   const double y_soft = 0.1;
   if (y_abs > y_soft) 
@@ -637,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) {
@@ -689,6 +683,27 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
+
+
+  Eigen::VectorXd y(2);
+  y << drive_cmd.x, drive_cmd.theta;
+
+  // Cập nhật lại A nếu dt thay đổi
+  for (int i = 0; i < n_; ++i)
+    for (int j = 0; j < n_; ++j)
+      A(i, j) = (i == j ? 1.0 : 0.0);
+  for (int i = 0; i < n_; i += 3)
+  {
+    A(i, i + 1) = dt;
+    A(i, i + 2) = 0.5 * dt * dt;
+    A(i + 1, i + 2) = dt;
+  }
+  kf_->update(y, dt, A);
+  double v_min = min_approach_linear_velocity_;
+  drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
+  drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
+  if (kf_filter_angular_)
+    drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -774,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
-  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);
+// #ifdef BUILD_WITH_ROS
+  // 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);
 
   // 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;
 }
 
@@ -1196,6 +1211,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
     const double v_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, remaining));
     v_limit = std::min(v_limit, v_stop);
   }
+  v_limit = std::min(v_limit, fabs(v_target));
 
   return std::copysign(v_limit, sign_x);
 }

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_end, my_end;
-    if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
-      start.pose.position.y,
-      mx_start, my_start)
+    // unsigned int mx_start, my_start;
+    // unsigned int mx_end, my_end;
+    // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
+    //   start.pose.position.y,
+    //   mx_start, my_start)
 
-      || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
-        goal.pose.position.y,
-        mx_end, my_end))
-    {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
-    }
+    //   || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
+    //     goal.pose.position.y,
+    //     mx_end, my_end))
+    // {
+    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
+    //   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)

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

@@ -101,6 +101,7 @@ add_library(${PROJECT_NAME} SHARED
   src/pnkx_local_planner.cpp
   src/pnkx_go_straight_local_planner.cpp
   src/pnkx_rotate_local_planner.cpp
+  src/pnkx_docking_local_planner.cpp
 )
 
 if(BUILDING_WITH_CATKIN)

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

@@ -1,8 +1,9 @@
 #ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 #define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 
-#include <robot_nav_core2/base_global_planner.h>
+#include <robot_nav_core/base_global_planner.h>
 #include <pnkx_local_planner/pnkx_local_planner.h>
+#include <robot_xmlrpcpp/XmlRpcValue.h>
 
 namespace pnkx_local_planner
 {
@@ -137,7 +138,7 @@ namespace pnkx_local_planner
       std::string docking_planner_name_;
       std::string docking_nav_name_;
 
-      robot_nav_core2::BaseGlobalPlanner::Ptr docking_planner_;
+      robot_nav_core::BaseGlobalPlanner::Ptr docking_planner_;
       score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
 
       robot_geometry_msgs::Vector3 linear_;
@@ -158,7 +159,7 @@ namespace pnkx_local_planner
       TFListenerPtr tf_;
       robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
       score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
-      std::function<robot_nav_core2::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
+      std::function<robot_nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
       std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
 
       robot::WallTimer detected_timeout_wt_, delayed_wt_;
@@ -169,7 +170,7 @@ namespace pnkx_local_planner
 
     bool start_docking_;
     double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
-    XmlRpc::XmlRpcValue original_papams_;
+    robot_xmlrpcpp::XmlRpcValue original_papams_;
     std::vector<DockingPlanner*> dkpl_;
 
     bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity);

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

@@ -56,102 +56,106 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     if (!costmap_robot_->isCurrent())
       throw robot_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());
-
+    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.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"));
+    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_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(traj_generator_name);
       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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
-      robot::NodeHandle nh_traj_gen = robot::NodeHandle(nh_, traj_generator_name);
+      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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_nav_name; 
-    planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+    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_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      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_ = algorithm_loader_();
+      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(nh_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
+      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;
-    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff"));
+    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_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
+      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_ = algorithm_loader_();
+      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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
-      rotate_algorithm_->initialize(nh_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
+      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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
+    robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
     try
     {
-      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";
-      boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(goal_checker_name);
+      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);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
-      goal_checker_->initialize(nh_);
+      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);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
+
     this->initializeOthers();
     this->getMaker();
     robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
@@ -162,8 +166,10 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
 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(""));
+  parent_.param("maker_name", maker_name, std::string(""));
+  parent_.param("maker_sources", maker_sources, std::string(""));
+  robot::log_info_at(__FILE__, __LINE__, "maker_name: %s", maker_name.c_str());
+  robot::log_info_at(__FILE__, __LINE__, "maker_sources: %s", maker_sources.c_str());
   std::stringstream ss(maker_sources);
   std::string source;
 
@@ -171,24 +177,23 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
   {
     if (maker_name == source)
     {
-      robot::NodeHandle source_node(nh_, source);
+      robot::NodeHandle source_node(parent_, source);
 
       if (source_node.hasParam("plugins"))
       {
-        XmlRpc::XmlRpcValue plugins;
+        robot_xmlrpcpp::XmlRpcValue plugins;
         source_node.getParam("plugins", plugins);
 
-        if (plugins.getType() == XmlRpc::XmlRpcValue::TypeArray)
+        if (plugins.getType() == robot_xmlrpcpp::XmlRpcValue::TypeArray)
         {
           for (int i = 0; i < plugins.size(); ++i)
           {
-            if (plugins[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
+            if (plugins[i].getType() == robot_xmlrpcpp::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;
@@ -215,7 +220,7 @@ 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_))
+  if (!parent_.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_;
@@ -245,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"));
-  robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
-  nh_.setParam(algorithm_nav_name, original_papams_);
+  // parent_.setParam(algorithm_nav_name, original_papams_);
+  robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
   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)
@@ -279,7 +289,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
   try
   {
-    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
   }
   catch(const robot_nav_core2::LocalPlannerException& e)
@@ -290,7 +300,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   double x_direction, y_direction, theta_direction;
   if (!ret_nav_)
   {
-    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -309,15 +319,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
       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);
+      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, 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_);
+      planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
 
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
       {
@@ -330,7 +340,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
             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))
+        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
         {
           throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
           robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
@@ -340,7 +350,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   }
   else
   {
-    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
@@ -388,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_)
@@ -396,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;
@@ -418,7 +435,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
   robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
-  robot_nav_2d_msgs::Path2D plan = transformed_plan_;
+  robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
   if (start_docking_)
   {
     local_goal = goal_pose_;
@@ -462,8 +479,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     // 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_);
+    robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+    parent_.setParam(algorithm_nav_name, original_papams_);
     nh_algorithm.setParam("allow_rotate", false);
   }
   return ret_nav_ && ret_angle_ && dock_ok;
@@ -479,7 +496,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
       {
         if (!dkpl_.front()->initialized_)
         {
-          dkpl_.front()->initialize(nh_, tf_, costmap_robot_, traj_generator_);
+          dkpl_.front()->initialize(parent_, tf_, costmap_robot_, traj_generator_);
           dkpl_.front()->initialized_ = true;
         }
         else
@@ -543,7 +560,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
         {
           if(!dkpl_.empty()) 
           {
-            if(dkpl_.front()) delete(dkpl_.front());
+            // if(dkpl_.front()) delete(dkpl_.front());
             dkpl_.erase(dkpl_.begin());
           }
           start_docking_ = false;
@@ -562,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),
-      is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
+    : initialized_(false),
+      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)
@@ -588,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";
-      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>(
+      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());
@@ -611,16 +662,17 @@ 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_, docking_nav_name_);
+      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_);
       robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str());
     }
@@ -657,13 +709,22 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   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);
+  robot::log_info("delay: %f", delay_time);
+  robot::log_info("timeout: %f", time_out);
+  robot::log_info("xy_goal_tolerance: %f", xy_goal_tolerance_);
+  robot::log_info("yaw_goal_tolerance: %f", yaw_goal_tolerance_);
+  robot::log_info("min_lookahead_dist: %f", min_lookahead_dist_);
+  robot::log_info("max_lookahead_dist: %f", max_lookahead_dist_);
+  robot::log_info("lookahead_time: %f", lookahead_time_);
+  robot::log_info("angle_threshold: %f", angle_threshold_);
+
+  detected_timeout_wt_ = ::robot::WallTimer(
+      ::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this, false, false);
   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_ = ::robot::WallTimer(
+      ::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this, false, false);
   delayed_wt_.start();
   robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
 

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/node_handle.h

@@ -283,6 +283,14 @@ namespace robot
        */
       bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const;
 
+      /**
+       * @brief Get a parameter as robot_xmlrpcpp::XmlRpcValue (converted from YAML).
+       * @param key The parameter key (supports nested keys with '/' separator).
+       * @param v Storage for the retrieved value. Left unchanged if key not found.
+       * @return true if the parameter was retrieved and converted successfully, false otherwise.
+       */
+      bool getParam (const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const;
+
       /**
        * @brief Template method to get a parameter value (without default).
        *

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

@@ -2,11 +2,17 @@
 #define ROBOT_ROBOT_H
 
 #include <robot/init.h>
-#include <robot/time.h>
-#include <robot/timer.h>
-#include <robot/rate.h>
 #include <robot/console.h>
 #include <robot/node_handle.h>
 #include <robot/plugin_loader_helper.h>
 
+
+#include <robot/time.h>
+#include <robot/timer.h>
+#include <robot/duration.h>
+#include <robot/wall_timer.h>
+#include <robot/rate.h>
+#include <robot/exception.h>
+#include <robot/macros.h>
+#include <robot/platform.h>
 #endif

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;
@@ -1727,6 +1728,87 @@ namespace robot
     return true;
   }
 
+  namespace
+  {
+    bool yamlToXmlRpc(const YAML::Node &y, robot_xmlrpcpp::XmlRpcValue &out)
+    {
+      if (!y.IsDefined())
+        return false;
+      if (y.IsScalar())
+      {
+        try
+        {
+          std::string s = y.as<std::string>();
+          std::string lower = s;
+          std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
+          if (lower == "true" || lower == "1" || lower == "yes" || lower == "on")
+          {
+            out = robot_xmlrpcpp::XmlRpcValue(true);
+            return true;
+          }
+          if (lower == "false" || lower == "0" || lower == "no" || lower == "off")
+          {
+            out = robot_xmlrpcpp::XmlRpcValue(false);
+            return true;
+          }
+          try
+          {
+            int i = y.as<int>();
+            out = robot_xmlrpcpp::XmlRpcValue(i);
+            return true;
+          }
+          catch (...) {}
+          try
+          {
+            double d = y.as<double>();
+            out = robot_xmlrpcpp::XmlRpcValue(d);
+            return true;
+          }
+          catch (...) {}
+          out = robot_xmlrpcpp::XmlRpcValue(s);
+          return true;
+        }
+        catch (...)
+        {
+          return false;
+        }
+      }
+      if (y.IsSequence())
+      {
+        out = robot_xmlrpcpp::XmlRpcValue();
+        out.setSize(static_cast<int>(y.size()));
+        for (size_t i = 0; i < y.size(); ++i)
+        {
+          robot_xmlrpcpp::XmlRpcValue item;
+          if (yamlToXmlRpc(y[i], item))
+            out[static_cast<int>(i)] = item;
+        }
+        return true;
+      }
+      if (y.IsMap())
+      {
+        out = robot_xmlrpcpp::XmlRpcValue();
+        for (YAML::const_iterator it = y.begin(); it != y.end(); ++it)
+        {
+          std::string k = it->first.as<std::string>();
+          robot_xmlrpcpp::XmlRpcValue item;
+          if (yamlToXmlRpc(it->second, item))
+            out[k] = item;
+        }
+        return true;
+      }
+      return false;
+    }
+  }
+
+  bool NodeHandle::getParam(const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const
+  {
+    YAML::Node value = getNestedValue(key);
+    if (!value.IsDefined())
+      return false;
+    return yamlToXmlRpc(value, v);
+  }
+
   template <typename T>
   T NodeHandle::param(const std::string &param_name) const
   {

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/CMakeLists.txt

@@ -19,6 +19,7 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 find_package(Boost REQUIRED COMPONENTS filesystem system)
 find_package(Threads REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(console_bridge REQUIRED)
 
 if (NOT BUILDING_WITH_CATKIN)
 
@@ -44,6 +45,7 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_nav_2d_utils
     robot_cpp
     robot_move_base_msgs
+    laser_filter
   )
   find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()
@@ -108,6 +110,7 @@ if(BUILDING_WITH_CATKIN)
     PRIVATE Boost::filesystem Boost::system
     dl
     ${TF3_LIBRARY}
+    ${console_bridge_LIBRARIES}
   )
 
 else()
@@ -123,6 +126,7 @@ else()
     PUBLIC
       ${PACKAGES_DIR}
       ${TF3_LIBRARY}
+      ${console_bridge_LIBRARIES}
     PRIVATE
       Boost::filesystem Boost::system
       yaml-cpp
@@ -149,6 +153,7 @@ if(BUILDING_WITH_CATKIN)
     Boost::filesystem Boost::system
     dl
     ${TF3_LIBRARY}
+    ${console_bridge_LIBRARIES}
   )
 else()
   target_link_libraries(${PROJECT_NAME}_main PRIVATE
@@ -157,6 +162,7 @@ else()
     Boost::filesystem Boost::system
     dl
     ${TF3_LIBRARY}
+    ${console_bridge_LIBRARIES}
   )
 
   # Configure RPATH to find libraries in devel space

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

@@ -31,6 +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),
@@ -268,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);
 
@@ -311,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);
@@ -321,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)
     {
@@ -397,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();
@@ -499,6 +505,20 @@ 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
+  robot_sensor_msgs::LaserScan laser_scan_filter = sor.filter(laser_scan);
+  if (it == laser_scans_.end())
+  {
+    laser_scans_[laser_scan_name] = laser_scan_filter;
+  }
+  else
+  {
+    it->second = laser_scan_filter;
+  }
+#else
   if (it == laser_scans_.end())
   {
     laser_scans_[laser_scan_name] = laser_scan;
@@ -507,6 +527,7 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   {
     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);
@@ -527,11 +548,15 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   // for (size_t i = 0; i < laser_scan.intensities.size(); i++) {
   //   intensities_str << laser_scan.intensities[i] << " ";
   // }
-  // robot::log_info("intensities: %s", intensities_str.str().c_str());
-  // robot::log_info("--------------------------------");
+  // 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)
@@ -807,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_)
@@ -958,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_)
@@ -1115,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_)
@@ -1191,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;
@@ -1228,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_)
@@ -1338,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_)
@@ -1461,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_)
@@ -1658,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)
@@ -1907,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);
@@ -2198,7 +2299,6 @@ void move_base::MoveBase::planThread()
     // if we didn't get a plan and we are in the planning state (the robot isn't moving)
     else if (state_ == move_base::PLANNING)
     {
-      robot::log_info("No Plan...");
       robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_);
 
       // check if we've tried to make a plan for over our time limit or our maximum number of retries
@@ -2259,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_);
@@ -2477,16 +2578,15 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
   // first we need to check if the quaternion has nan's or infs
   if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
   {
-    std::cerr << "Quaternion has nans or infs... discarding as a navigation goal" << std::endl;
+    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
   if (tf_q.length2() < 1e-6)
   {
-    std::cerr << "Quaternion has length close to zero... discarding as navigation goal" << std::endl;
+    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
     return false;
   }
 
@@ -2499,7 +2599,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
 
   if (fabs(dot - 1) > 1e-3)
   {
-    std::cerr << "Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical." << std::endl;
+    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
     return false;
   }
 
@@ -2541,9 +2641,7 @@ bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_
   if (!global_pose.header.stamp.isZero() &&
       current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
   {
-    std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". "
-                                                                           "Current time: "
-              << current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl;
+    robot::log_error("Transform timeout for %s. Current time: %f, pose stamp: %f, tolerance: %f", costmap->getName().c_str(), current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
     return false;
   }
 
@@ -2644,6 +2742,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       recovery_index_ = 0;
   }
 
+
   // Cancle executeCycle
   if (cancel_ctr_ && tc_)
   {
@@ -2652,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;
@@ -2669,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;
       }
     }
@@ -2678,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_);
@@ -2690,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;
     }
@@ -2710,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
@@ -2884,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;
           }
@@ -2988,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());