fix load path so

.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

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

config/costmap_common_params.yaml

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

config/dock_global_params.yaml

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

config/move_base_common_params.yaml

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

config/pnkx_local_planner_params.yaml

@@ -41,8 +41,8 @@ PNKXRotateLocalPlanner:
     
 LimitedAccelGenerator:
   library_path: libmkt_plugins_standard_traj_generator
-  max_vel_x: 0.2
+  max_vel_x: 1.2
-  min_vel_x: -0.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"
 
@@ -152,6 +152,10 @@ namespace NavigationExample
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, string marker, PoseStamped goal);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_nodes_edges(NavigationHandle handle, string marker, Node[] nodes, Edge[] edges, PoseStamped goal);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);

examples/NavigationExample/Program.cs

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

examples/run_example.sh

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

src/APIs/c_api/include/nav_c_api.h

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

src/APIs/c_api/src/nav_c_api.cpp

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

src/Algorithms/Libraries/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

@@ -412,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

@@ -35,6 +35,12 @@ mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
     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)
 {
@@ -83,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]
+    // State: [v, a, j, w, alpha, jw] where:
-    m_ = 2; // measurements: x, y, theta
+    //  - 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_);
+    R = Eigen::MatrixXd::Zero(m_, m_);
-    P = Eigen::MatrixXd::Identity(n_, n_);
+    P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_);
 
     for (int i = 0; i < n_; i += 3)
     {
@@ -103,15 +113,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     C(0, 0) = 1;
     C(1, 3) = 1;
-    Q(2, 2) = 0.1;
+    Q(2, 2) = std::max(1e-12, kf_q_v_);
-    Q(5, 5) = 0.6;
+    Q(5, 5) = std::max(1e-12, kf_q_w_);
 
-    R(0, 0) = 0.1;
+    R(0, 0) = std::max(1e-12, kf_r_v_);
-    R(1, 1) = 0.2;
+    R(1, 1) = std::max(1e-12, kf_r_w_);
-
-    P(3, 3) = 0.4;
-    P(4, 4) = 0.4;
-    P(5, 5) = 0.4;
 
     kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
     Eigen::VectorXd x0(n_);
@@ -177,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;
 
@@ -558,25 +573,6 @@ 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());
@@ -606,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);
 
@@ -635,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) {
@@ -687,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(
@@ -772,16 +789,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   //   (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
 
   bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
-#ifdef BUILD_WITH_ROS
+// #ifdef BUILD_WITH_ROS
-  if (result)
+  // if (result)
-    ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+  //   ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
 
   // else if(fabs(velocity.x) < min_speed_xy_)
   // {
   //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
   //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
   // }
-#endif
+// #endif
   return result;
 }
 

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

@@ -36,8 +36,6 @@ namespace two_points_planner
     if (!initialized_)
     {
       robot::NodeHandle nh_priv_("~/" + name);
-      robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());
-
       int lethal_obstacle;
       nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
       lethal_obstacle_ = (unsigned char)lethal_obstacle;
@@ -121,6 +119,7 @@ namespace two_points_planner
       robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
       return false;
     }
+    
     robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start);
     robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);
     robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", 
@@ -148,21 +147,21 @@ namespace two_points_planner
     }
 
     plan.clear();
-    plan.push_back(start);
+    // plan.push_back(start);
     
-    unsigned int mx_start, my_start;
+    // unsigned int mx_start, my_start;
-    unsigned int mx_end, my_end;
+    // unsigned int mx_end, my_end;
-    if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
+    // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
-      start.pose.position.y,
+    //   start.pose.position.y,
-      mx_start, my_start)
+    //   mx_start, my_start)
 
-      || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
+    //   || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
-        goal.pose.position.y,
+    //     goal.pose.position.y,
-        mx_end, my_end))
+    //     mx_end, my_end))
-    {
+    // {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
+    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
+    //   return false;
-    }
+    // }
     // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
     // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
     // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)

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

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

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

@@ -80,7 +80,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_nav_name = std::string("pnkx_local_planner::GoStraight");
@@ -104,14 +104,14 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string goal_checker_name;
@@ -126,7 +126,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
       goal_checker_->initialize(nh_goal_checker);
@@ -134,7 +134,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str());

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

@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -97,7 +97,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
     
     std::string algorithm_nav_name; 
@@ -107,20 +107,21 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str());
       nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
       nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
@@ -135,7 +136,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
       rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@@ -143,7 +144,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
@@ -159,7 +160,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -167,7 +168,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     this->initializeOthers();

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

@@ -64,7 +64,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -72,7 +72,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
       nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
@@ -96,7 +96,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string goal_checker_name;
@@ -111,7 +111,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
       goal_checker_->initialize(nh_goal_checker);
@@ -119,7 +119,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
     
     robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());

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

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

src/Libraries/robot_cpp/src/node_handle.cpp

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

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

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

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

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

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

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

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

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

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -45,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()

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),
@@ -321,6 +324,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)
     {
@@ -499,6 +503,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 +525,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);
@@ -529,8 +548,13 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   // }
   // robot::log_error("intensities: %s", intensities_str.str().c_str());
 
+#ifndef BUILD_WITH_ROS
+  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+#else
   updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
   updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+#endif
 }
 
 robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)
@@ -807,7 +831,17 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
 {
   if (setup_)
   {
-    swapPlanner(default_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);
+    }
   }
   else
   {
@@ -958,7 +992,17 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
 {
   if (setup_)
   {
-    swapPlanner(default_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);
+    }
   }
   else
   {
@@ -1115,7 +1159,17 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
 
   if (setup_)
   {
-    swapPlanner(default_config_.base_global_planner);
+    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
   {
@@ -1190,6 +1244,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,7 +1323,17 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
 
   if (setup_)
   {
-    swapPlanner(default_config_.base_global_planner);
+    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
   {
@@ -1337,7 +1442,17 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
-    swapPlanner(global_planner);
+    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
   {
@@ -1460,7 +1575,17 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
-    swapPlanner(global_planner);
+    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
   {
@@ -1657,32 +1782,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)
@@ -2257,8 +2356,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_);
@@ -2478,7 +2578,6 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
     robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
     return false;
   }
-
   tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
 
   // next, we need to check if the length of the quaternion is close to zero
@@ -2640,6 +2739,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       recovery_index_ = 0;
   }
 
+
   // Cancle executeCycle
   if (cancel_ctr_ && tc_)
   {
@@ -2648,13 +2748,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)
-        swapPlanner(default_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;
@@ -2665,7 +2777,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;
       }
     }
@@ -2674,7 +2800,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)
-      swapPlanner(default_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_);
@@ -2686,6 +2822,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;
     }
@@ -2706,7 +2856,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
@@ -2880,7 +3029,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;
           }
@@ -2984,15 +3134,48 @@ 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)
-    tf3::doTransform(goal_pose, global_pose, transform);
+    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)
   {