update rotate to goal

config/move_base_common_params.yaml

@@ -4,7 +4,7 @@ controller_frequency: 30.0   # run controller at 15.0 Hz
 controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
 planner_frequency: 0.0       # don't continually replan (only when controller failed)
 planner_patience: 2.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
-max_planning_retries: 5     # ... or after 10 attempts (whichever happens first)
+max_planning_retries: 0     # ... or after 10 attempts (whichever happens first)
 oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
 oscillation_distance: 0.5
 ### recovery behaviors

config/pnkx_local_planner_params.yaml

@@ -111,8 +111,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
   avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
+  xy_local_goal_tolerance: 0.02
+  angle_threshold: 0.8
   index_samples: 60
   follow_step_path: true
 
@@ -121,42 +121,12 @@ MKTAlgorithmDiffGoStraight:
   # only when false:
   lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  min_lookahead_dist: 0.325                    # The minimum lookahead distance (m) threshold. (default: 0.3)
-  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  min_lookahead_dist: 1.0                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
   lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.6                    # Minimum squared journey to consider for goal (default: 0.2) 
-
-  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
-  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
-  # only when true:
-  rotate_to_heading_min_angle: 0.35   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
-
-  # stoped
-  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.06
-  
-  use_regulated_linear_velocity_scaling: false
-  use_cost_regulated_linear_velocity_scaling: false
-
-MKTAlgorithmDiffRotateToGoal:
-  library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
-  index_samples: 60
-  follow_step_path: true
-
-  # Lookahead
-  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
-  # only when false:
-  lookahead_dist: 0.325                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
-  # only when true:
-  min_lookahead_dist: 0.8                     # The minimum lookahead distance (m) threshold. (default: 0.3)
-  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.6                    # Maximum squared journey to consider for goal (default: 0.2) 
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
   # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
   use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
@@ -165,24 +135,53 @@ MKTAlgorithmDiffRotateToGoal:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.03
+  rot_stopped_velocity: 0.05
   trans_stopped_velocity: 0.06
   
-  # Regulated linear velocity scaling
-  use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
+  use_final_heading_alignment: true
+  final_heading_xy_tolerance: 0.1
+  final_heading_angle_tolerance: 0.05
+  final_heading_min_velocity: 0.05
+  final_heading_kp_angular: 2.0
+
+MKTAlgorithmDiffRotateToGoal:
+  library_path: libmkt_algorithm_diff
+  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.02
+  angle_threshold: 0.47
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
-  regulated_linear_scaling_min_speed: 0.05      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
-  # Inflation cost scaling (Limit velocity by proximity to obstacles)
-  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
-  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
-  cost_scaling_dist: 0.2                             # (default: 0.6)
-  cost_scaling_gain: 2.0                             # (default: 1.0)
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+  angular_decel_zone: 0.1             
 
+  # stoped
+  rot_stopped_velocity: 0.05
+  trans_stopped_velocity: 0.06
+  
+  use_final_heading_alignment: true
+  final_heading_xy_tolerance: 0.1
+  final_heading_angle_tolerance: 0.05
+  final_heading_min_velocity: 0.05
+  final_heading_kp_angular: 2.0
   
 GoalChecker:
   library_path: libmkt_plugins_goal_checker
 
 SimpleGoalChecker:
-  library_path: libmkt_plugins_simple_goal_checker
+  library_path: libmkt_plugins_goal_checker

examples/CSharpExample.cs

@@ -469,14 +469,6 @@ namespace NavigationExample
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data);
 
-
-        // ============================================================================
-        // Navigation Commands with Order
-        // ============================================================================
-
-
-
-
         // ============================================================================
         // Helper Methods for String Conversion
         // ============================================================================

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

@@ -108,16 +108,101 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   {
     twist = traj_->nextTwist();
   }
-  
-  drive_cmd.x = sqrt(twist.x * twist.x);
-  robot_nav_2d_msgs::Path2D transformed_plan = transform_plan_;
+  double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
+  robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
-    robot::log_warning("Transformed plan is empty. Cannot determine a local plan.");
+    robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
+    stopWithAccLimits(pose, velocity, drive_cmd, dt);
+    result.velocity = drive_cmd;
+    prevous_drive_cmd_ = drive_cmd;
+    return result;
+  }
+  double lookahead_dist = getLookAheadDistance(velocity);
+  auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
+  robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
+  
+  // === Final Heading Alignment Check ===
+  double xy_error = 0.0, heading_error = 0.0;
+  if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
+  {
+    // Use Arc Motion controller for final heading alignment
+    alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
+#ifdef BUILD_WITH_ROS
+    ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+                      xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+#endif
+  }
+  else
+  {
+    if(fabs(carrot_pose.pose.y) > 0.2)
+    {
+      lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
+    }
+    robot_nav_2d_msgs::Twist2D drive_target;
+    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+    carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
+
+    // Normal Pure Pursuit
+    this->computePurePursuit(
+    carrot_pose, 
+    drive_target,
+    velocity, 
+    transformed_plan,
+    min_approach_linear_velocity_, 
+    sign_x, 
+    dt, 
+    drive_cmd);
+  }
+  applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
+
+  if (this->nav_stop_)
+  {
+    if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
+    {
+      if (!stopWithAccLimits(pose, velocity, drive_cmd, dt))
+        return result;
+    }
+    else
+      drive_cmd = {};
+    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)
+  {
+    result.poses.push_back(pose_stamped.pose);
+    if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 && 
+      fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 && 
+      fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
+    break;
+  }
+
+  result.velocity = drive_cmd;
+  prevous_drive_cmd_ = drive_cmd;
   return result;
 }
 

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

@@ -627,7 +627,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
-  robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta);
+  // robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(

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

@@ -33,6 +33,8 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_nav_core
     robot_geometry_msgs
     robot_nav_msgs
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
     tf3
     robot_tf3_geometry_msgs
     robot_time
@@ -49,6 +51,8 @@ else()
     robot_nav_core
     robot_geometry_msgs
     robot_nav_msgs
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
     tf3
     robot_tf3_geometry_msgs
     robot_time

src/Algorithms/Packages/global_planners/two_points_planner/package.xml

@@ -31,6 +31,12 @@
   <build_depend>robot_nav_msgs</build_depend>
   <run_depend>robot_nav_msgs</run_depend>
 
+  <build_depend>robot_nav_2d_msgs</build_depend>
+  <run_depend>robot_nav_2d_msgs</run_depend>
+
+  <build_depend>robot_nav_2d_utils</build_depend>
+  <run_depend>robot_nav_2d_utils</run_depend>
+
   <build_depend>tf3</build_depend>
   <run_depend>tf3</run_depend>
 

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

@@ -3,6 +3,8 @@
 #include <robot_costmap_2d/inflation_layer.h>
 #include <boost/dll/alias.hpp>
 #include <robot_nav_msgs/Path.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_2d_utils/conversions.h>
 #include <tf3/LinearMath/Quaternion.h>
 #include <tf3/utils.h>
 #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
@@ -49,6 +51,8 @@ namespace two_points_planner
       initialized_ = true;
       return true;
     }
+    robot::log_info("[%s:%d]\n TwoPointsPlanner: Already initialized", __FILE__, __LINE__);
+    return true;
   }
 
   unsigned char TwoPointsPlanner::computeCircumscribedCost()
@@ -104,22 +108,26 @@ namespace two_points_planner
   {
     if (!initialized_)
     {
-      std::cerr << "TwoPointsPlanner: Global planner is not initialized" << std::endl;
+      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)", 
+      start_2d.pose.x, start_2d.pose.y, start_2d.pose.theta, goal_2d.pose.x, goal_2d.pose.y, goal_2d.pose.theta);
 
     bool do_init = false;
     if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
       current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY())
     {
-      printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n",
+      robot::log_warning("[%s:%d]\n TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.", __FILE__, __LINE__,
         current_env_width_, current_env_height_,
         costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
       do_init = true;
     }
     else if (footprint_ != costmap_robot_->getRobotFootprint())
     {
-      printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n");
+      robot::log_warning("[%s:%d]\n TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.", __FILE__, __LINE__);
       do_init = true;
     }
 
@@ -131,8 +139,6 @@ namespace two_points_planner
 
     plan.clear();
     plan.push_back(start);
-    printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n",
-      start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
     
     unsigned int mx_start, my_start;
     unsigned int mx_end, my_end;
@@ -144,7 +150,7 @@ namespace two_points_planner
         goal.pose.position.y,
         mx_end, my_end))
     {
-      std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl;
+      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));
@@ -152,7 +158,7 @@ namespace two_points_planner
     if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
       || end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
     {
-      std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
+      robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__);
       return false;
     }
     robot::Time plan_time = robot::Time::now();
@@ -171,7 +177,7 @@ namespace two_points_planner
     }
     else 
     {
-      std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl;
+      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
       return false;
     }
 

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

@@ -51,17 +51,18 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     
     parent_ = parent;
     planner_nh_ = robot::NodeHandle(parent_, name);
-    
+    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_);
  
     // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window
     std::string traj_generator_name;
     planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
-
     robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator  \"%s\"", traj_generator_name.c_str());
+    
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_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_();
@@ -84,7 +85,8 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     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";
+      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_ = nav_algorithm_loader_();
@@ -106,7 +108,8 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("score_algorithm::GoalChecker"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_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(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_();

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

@@ -58,6 +58,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
 
     parent_ = parent;
     planner_nh_ = robot::NodeHandle(parent_, name);
+    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;

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

@@ -55,7 +55,8 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     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_();
@@ -75,19 +76,21 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
 
     std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
     planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate"));
+    robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_rotate_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";
-      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
+      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
-      rotate_algorithm_ = rotate_algorithm_loader_();
+      nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
         exit(1);
       }
       robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
-      rotate_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
+      nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception& ex)
     {
@@ -99,7 +102,8 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("nav_algorithm::GoalChecker"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_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(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_();
@@ -126,10 +130,13 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::reset()
 {
   robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
   this->unlock();
+  if (traj_generator_)
     traj_generator_->reset();
+  if (goal_checker_)
     goal_checker_->reset();
+  if (nav_algorithm_)
     nav_algorithm_->reset();
-  ret_nav_ = false;
+  ret_nav_ = ret_angle_ = false;
 }
 
 void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity)
@@ -147,12 +154,10 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
     if (!costmap_robot_->isCurrent())
       throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
   }
-
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
     local_goal_pose = this->transformPoseToLocal(goal_pose_);
-
   try
   {
     if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))

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

@@ -198,6 +198,7 @@ namespace robot_nav_core_adapter
     boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
     robot_nav_core2::LocalPlanner::Ptr planner_;
     
+    bool initialized_;
     // Pointer Copies
     TFListenerPtr tf_;
 
@@ -211,7 +212,6 @@ namespace robot_nav_core_adapter
 
     robot_nav_core_adapter::NavCoreAdapterConfig last_config_;
     robot_nav_core_adapter::NavCoreAdapterConfig default_config_;
-    bool setup_;
   };
 
 } // namespace robot_nav_core_adapter

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

@@ -74,6 +74,8 @@ namespace robot_nav_core_adapter
    * @brief Load the robot_nav_core2 local planner and initialize it
    */
   void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
+  {
+    if (!initialized_)
     {
       tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
       costmap_robot_ = costmap_robot;
@@ -105,6 +107,7 @@ namespace robot_nav_core_adapter
           robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str());
           exit(EXIT_FAILURE);
         }
+        last_config_.planner_name = planner_name;
         robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str());
         planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
         robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str());
@@ -119,43 +122,57 @@ namespace robot_nav_core_adapter
         robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what());
           exit(EXIT_FAILURE);
       }
+      
+      initialized_ = true;
+      robot::log_info("LocalPlannerAdapter initialized successfully");
+    }
+    else
+    {
+      robot::log_info("LocalPlannerAdapter already initialized");
+    }
   }
 
   bool LocalPlannerAdapter::swapPlanner(std::string planner_name)
   {
     boost::recursive_mutex::scoped_lock l(configuration_mutex_);
-    if (!setup_)
+    if (!initialized_)
     {
       return false;
     }
-
     if (planner_name != last_config_.planner_name)
     {
-      robot_nav_core2::LocalPlanner::Ptr new_planner;
-
+      robot_nav_core2::LocalPlanner::Ptr old_planner = planner_;
+      auto old_loader = planner_loader_;
       try
       {
-        // Tạo planner mới
-        std::string path_file_so;
-        planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
+        boost::unique_lock<boost::recursive_mutex> lock(configuration_mutex_);
+        robot::PluginLoaderHelper loader;
+        std::string path_file_so = loader.findLibraryPath(planner_name);
+        auto new_loader = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
           path_file_so, planner_name, boost::dll::load_mode::append_decorations);
-        new_planner = planner_loader_();
+
+        auto new_planner = new_loader();
         if (!new_planner)
         {
-          std::cerr << "Failed to load planner " << planner_name << std::endl;
-          exit(EXIT_FAILURE);
+          robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str());
+          throw std::runtime_error("Failed to load planner " + planner_name);
         }
-        new_planner->initialize(adapter_nh_, planner_name, tf_, costmap_robot_);
-        if (planner_)
-          planner_.reset();
+        new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
         planner_ = new_planner;
-
+        planner_loader_ = new_loader;
         last_config_.planner_name = planner_name;
         robot::log_info("Loaded new planner: %s", planner_name.c_str());
+
+        // Release old planner while old loader is still alive.
+        old_planner.reset();
+        old_loader = boost::function<robot_nav_core2::LocalPlanner::Ptr()>();
+        lock.unlock();
       }
       catch (const std::exception &ex)
       {
-        robot::log_error("Failed to load planner %s: %s", planner_name.c_str(), ex.what());
+        robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: %s", planner_name.c_str(), ex.what());
+        planner_ = old_planner;
+        planner_loader_ = old_loader;
         return false;
       }
     }

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -90,6 +90,10 @@ add_library(${PROJECT_NAME} SHARED
 if(BUILDING_WITH_CATKIN)
   add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+  target_compile_definitions(${PROJECT_NAME}
+    PUBLIC BUILD_WITH_ROS
+  )
+
   target_include_directories(${PROJECT_NAME}
     PUBLIC
       $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

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

@@ -4,6 +4,9 @@
  *
  * Author: Kevin Lee
  *********************************************************************/
+#ifdef BUILD_WITH_ROS
+#include <ros/ros.h>
+#endif
 #include <move_base/move_base.h>
 #include <move_base/convert_data.h>
 #include <cmath>
@@ -231,6 +234,8 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
         robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
         throw std::runtime_error("Failed to load global planner " + global_planner);
       }
+      last_config_.base_global_planner = global_planner;
+      default_config_.base_global_planner = global_planner;
       if (planner_->initialize(global_planner, planner_costmap_robot_))
         robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__);
       else
@@ -333,7 +338,54 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
 
 void move_base::MoveBase::swapPlanner(std::string base_global_planner)
 {
+  if (base_global_planner != last_config_.base_global_planner)
+  {
+    robot_nav_core::BaseGlobalPlanner::Ptr old_planner = planner_;
+    auto old_loader = planner_loader_;
+    // initialize the global planner
+    try
+    {
+      // wait for the current planner to finish planning
+      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(base_global_planner);
 
+      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();
+      if (!new_planner)
+      {
+        robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr", __FILE__, __LINE__);
+        throw std::runtime_error("Failed to load global planner " + base_global_planner);
+      }
+
+      if (planner_plan_)
+        planner_plan_->clear();
+      if (latest_plan_)
+        latest_plan_->clear();
+      if (controller_plan_)
+        controller_plan_->clear();
+      resetState();
+
+      new_planner->initialize(base_global_planner, planner_costmap_robot_);
+
+      planner_ = new_planner;
+      planner_loader_ = new_loader;
+      last_config_.base_global_planner = base_global_planner;
+
+      // Release old planner while old loader is still alive.
+      old_planner.reset();
+      old_loader = boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()>();
+      lock.unlock();
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error("[%s:%d]\n Failed to create the %s planner, are you sure it is properly registered and that the \
+                  containing library is built? Exception: %s",__FILE__, __LINE__, base_global_planner.c_str(), ex.what());
+      planner_ = old_planner;
+      planner_loader_ = old_loader;
+    }
+  }
 }
 
 void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt)
@@ -788,8 +840,9 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
   }
   catch (const std::exception &e)
   {
-    robot::log_error("[MoveBase] Exception in swapPlanner: %s", e.what());
-    throw std::exception(e);
+    robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what());
+    lock.unlock();
+    return false;
   }
 
   if (nav_feedback_)
@@ -897,8 +950,9 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
   }
   catch (const std::exception &e)
   {
-    std::cerr << e.what() << "\n";
-    throw std::exception(e);
+    robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what());
+    lock.unlock();
+    return false;
   }
 
   if (nav_feedback_)
@@ -923,6 +977,7 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
 
 bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
 {
+  robot::log_info("[MoveBase::moveStraightTo] Entry");
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
@@ -938,12 +993,10 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
     }
     return false;
   }
-
   if (fabs(xy_goal_tolerance) > 0.001)
     this->setXyGoalTolerance(fabs(xy_goal_tolerance));
   else
     this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));
-
   if (!tc_)
   {
     throw std::runtime_error("Null 'tc_' pointer encountered");
@@ -954,7 +1007,17 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
   }
 
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+
+  try
+  {
     tc_->swapPlanner(go_straight_planner_name_);
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[MoveBase::moveStraightTo] Exception in swapPlanner: %s", e.what());
+    lock.unlock();
+    return false;
+  }
 
   if (nav_feedback_)
   {
@@ -965,6 +1028,48 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
   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::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
+    robot::log_info("[MoveBase::moveStraightTo] 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::moveStraightTo] Processing goal through action server...");
+    as_->processGoal(action_goal);
+    robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server");
+  }
+  catch (const std::exception &e)
+  {
+    lock.unlock();
+    robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
+    return false;
+  }
+
   lock.unlock();
   return true;
 }
@@ -987,7 +1092,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
     return false;
   }
 
-  this->setXyGoalTolerance(0.25);
+  this->setXyGoalTolerance(std::numeric_limits<double>::infinity());
 
   if (fabs(yaw_goal_tolerance) > 0.001)
     this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
@@ -1003,7 +1108,17 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+
+  try
+  {
     tc_->swapPlanner(rotate_planner_name_);
+  }
+  catch (const std::exception &e)
+  {
+    robot::log_error("[MoveBase::rotateTo] Exception in swapPlanner: %s", e.what());
+    lock.unlock();
+    return false;
+  }
 
   if (nav_feedback_)
   {
@@ -1013,7 +1128,19 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
   }
   if (cancel_ctr_)
     cancel_ctr_ = false;
-  robot_geometry_msgs::PoseStamped pose;
+
+  // Check if action server exists
+  if (!as_)
+  {
+    robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
+    lock.unlock();
+    return false;
+  }
+
+  try
+  {
+
+    robot_geometry_msgs::Pose2D pose;
     if (!this->getRobotPose(pose))
     {
       if (nav_feedback_)
@@ -1024,6 +1151,42 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
       }
       return false;
     }
+
+    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;
+    double distance = planner_costmap_robot_ ? planner_costmap_robot_->getCostmap()->getResolution() : 0.05;
+    action_goal->goal.target_pose.pose.position.x = pose.x + distance * cos(pose.theta);
+    action_goal->goal.target_pose.pose.position.y = pose.y + distance * sin(pose.theta);
+
+    // 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::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
+    robot::log_info("[MoveBase::rotateTo] 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::rotateTo] Processing goal through action server...");
+    as_->processGoal(action_goal);
+    robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server");
+  }
+  catch (const std::exception &e)
+  {
+    lock.unlock();
+    robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
+    return false;
+  }
+
   lock.unlock();
   return true;
 }