update

.gitignore

@@ -422,3 +422,4 @@ build
 install
 devel
 
+obstacle

CMakeLists.txt

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

config/costmap_common_params.yaml

@@ -19,17 +19,33 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing 
+  observation_sources:  l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing
-  f_scan_marking:
+  l_scan_marking:
-    topic: /f_scan
+    topic: /l_scan
     data_type: LaserScan
     clearing: false
     marking: true
     inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
-  f_scan_clearing:
+  l_scan_clearing:
-    topic: /f_scan
+    topic: /l_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_marking:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_clearing:
+    topic: /r_scan
     data_type: LaserScan
     clearing: true
     marking: false
@@ -52,5 +68,6 @@ obstacles:
     inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
+  
 
 

config/costmap_global_params.yaml

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

config/costmap_local_params.yaml

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

config/hybrid_local_planner_params.yaml

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

config/maker_sources.yaml

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

examples/NavigationExample/NavigationExample.csproj

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

src/APIs/c_api/src/nav_c_api.cpp

@@ -83,7 +83,6 @@ 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();

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

@@ -174,9 +174,8 @@ namespace two_points_planner
     // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
     const double dx = goal.pose.position.x - start.pose.position.x;
     const double dy = goal.pose.position.y - start.pose.position.y;
-    const double distance = std::sqrt(dx * dx + dy * dy);
+    double distance = std::sqrt(dx * dx + dy * dy);
     double theta;
-
     if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
     {
       theta = std::atan2(dy, dx);
@@ -187,13 +186,9 @@ namespace two_points_planner
       if(cos(goal_theta - theta) < 0) theta += M_PI;
     }
     else 
-    { 
+    {
-      robot_geometry_msgs::PoseStamped pose = start;
+      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
-      pose.pose.position.x += 0.01 * cos(theta);
+      return false;
-      pose.pose.position.y += 0.01 * sin(theta);
-      plan.push_back(pose);
-      plan.push_back(goal);
-      return true;
     }
 
     // Lấy độ phân giải của costmap

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

@@ -52,12 +52,6 @@ namespace pnkx_local_planner
      */
     void getPlan(robot_nav_2d_msgs::Path2D &path) override;
 
-    /**
-     * @brief robot_nav_core2 getGlobalPlan - Gets the current global plan
-     * @param path The global plan
-     */
-    void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) override;
-
     /**
      * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *

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

@@ -254,8 +254,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
   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_);
-  parent_.setParam(algorithm_nav_name, original_papams_);
   robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
   nh_algorithm.setParam("allow_rotate", false);
 
@@ -327,8 +326,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
       nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
       nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
 
-      parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
-      parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+      planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
 
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
       {

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

@@ -235,15 +235,6 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa
 
 }
 
-void pnkx_local_planner::PNKXLocalPlanner::getGlobalPlan(robot_nav_2d_msgs::Path2D &path)
-{
-  if (global_plan_.poses.empty())
-  {
-    return;
-  }
-  path = global_plan_;
-}
-
 void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
   this->getParams(planner_nh_);

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

@@ -89,7 +89,7 @@ public:
 
 private:
   /**
-   * @brief Resolve library path (absolute, then PNKX_NAV_CORE_LIBRARY_PATH, search_paths_, cwd)
+   * @brief Resolve library path (handle relative paths, search in search_paths)
    * @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

@@ -16,70 +16,6 @@
 #endif
 #include <robot_xmlrpcpp/XmlRpcValue.h>
 
-namespace
-{
-
-  static YAML::Node xmlRpcToYaml(robot_xmlrpcpp::XmlRpcValue v)
-  {
-    using robot_xmlrpcpp::XmlRpcValue;
-    switch (v.getType())
-    {
-    case XmlRpcValue::TypeBoolean:
-    {
-      bool b = v;
-      return YAML::Node(b);
-    }
-    case XmlRpcValue::TypeInt:
-    {
-      int i = v;
-      return YAML::Node(i);
-    }
-    case XmlRpcValue::TypeDouble:
-    {
-      double d = v;
-      return YAML::Node(d);
-    }
-    case XmlRpcValue::TypeString:
-    {
-      std::string s = v;
-      return YAML::Node(s);
-    }
-    case XmlRpcValue::TypeArray:
-    {
-      YAML::Node seq(YAML::NodeType::Sequence);
-      for (int i = 0; i < v.size(); ++i)
-        seq.push_back(xmlRpcToYaml(v[i]));
-      return seq;
-    }
-    case XmlRpcValue::TypeStruct:
-    {
-      YAML::Node map(YAML::NodeType::Map);
-      const XmlRpcValue::ValueStruct *members = v.getStructMembers();
-      if (members)
-      {
-        for (const auto &kv : *members)
-          map[kv.first] = xmlRpcToYaml(kv.second);
-      }
-      return map;
-    }
-    default:
-      return YAML::Node();
-    }
-  }
-
-  static YAML::NodeType::value yamlNodeCategory(const YAML::Node &n)
-  {
-    if (n.IsMap())
-      return YAML::NodeType::Map;
-    if (n.IsSequence())
-      return YAML::NodeType::Sequence;
-    if (n.IsScalar())
-      return YAML::NodeType::Scalar;
-    return YAML::NodeType::Null;
-  }
-
-} // namespace
-
 namespace robot
 {
 
@@ -1211,14 +1147,92 @@ namespace robot
 
   void NodeHandle::setParam(const std::string &key, const robot_xmlrpcpp::XmlRpcValue &v) const
   {
+    // Convert XmlRpcValue to YAML::Node
+    // Create non-const copy to use conversion operators
+    robot_xmlrpcpp::XmlRpcValue v_copy = v;
+    YAML::Node node;
     try
     {
-      robot_xmlrpcpp::XmlRpcValue v_copy = v;
+      switch (v.getType())
-      YAML::Node node = xmlRpcToYaml(v_copy);
+      {
-      const_cast<NodeHandle *>(this)->setParamInternal(key, node, yamlNodeCategory(node));
+      case robot_xmlrpcpp::XmlRpcValue::TypeBoolean:
+      {
+        bool b = v_copy;
+        node = YAML::Node(b);
+        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
+      }
+      break;
+      case robot_xmlrpcpp::XmlRpcValue::TypeInt:
+      {
+        int i = v_copy;
+        node = YAML::Node(i);
+        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
+      }
+      break;
+      case robot_xmlrpcpp::XmlRpcValue::TypeDouble:
+      {
+        double d = v_copy;
+        node = YAML::Node(d);
+        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
+      }
+      break;
+      case robot_xmlrpcpp::XmlRpcValue::TypeString:
+      {
+        std::string s = v_copy;
+        node = YAML::Node(s);
+        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
+      }
+      break;
+      case robot_xmlrpcpp::XmlRpcValue::TypeArray:
+        {
+        YAML::Node seq(YAML::NodeType::Sequence);
+        for (int i = 0; i < v_copy.size(); ++i)
+        {
+          YAML::Node item;
+          robot_xmlrpcpp::XmlRpcValue item_v = v_copy[i];
+          if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeBoolean)
+          {
+            bool b = item_v;
+            item = YAML::Node(b);
+          }
+          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt)
+          {
+            int i_val = item_v;
+            item = YAML::Node(i_val);
+          }
+          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeDouble)
+          {
+            double d = item_v;
+            item = YAML::Node(d);
+          }
+          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeString)
+          {
+            std::string s = item_v;
+            item = YAML::Node(s);
+          }
+          seq.push_back(item);
+        }
+        const_cast<NodeHandle *>(this)->setParamInternal(key, seq, YAML::NodeType::Sequence);
+      }
+      break;
+      case robot_xmlrpcpp::XmlRpcValue::TypeStruct:
+      {
+        YAML::Node map_node(YAML::NodeType::Map);
+        // XmlRpcValue::TypeStruct doesn't have begin/end, need to use different approach
+        // We'll need to iterate through the struct differently
+        // For now, create empty map
+        const_cast<NodeHandle *>(this)->setParamInternal(key, map_node, YAML::NodeType::Map);
+      }
+      break;
+      default:
+        // Unknown type, create empty node
+        const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
+        break;
+      }
     }
     catch (...)
     {
+      // On error, create empty node
       const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
     }
   }

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

@@ -98,6 +98,7 @@ 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,27 +199,21 @@ std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_pa
     return "";
   }
 
-  // If relative path, search under PNKX_NAV_CORE_LIBRARY_PATH (':'-separated, like LD_LIBRARY_PATH)
+  // If relative path, search in search_paths (build directory is already added)
-  const char* nav_lib_path_env = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
+  std::string build_dir = getBuildDirectory();
-  if (nav_lib_path_env) {
+  if (!build_dir.empty()) {
+    // 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::string nav_lib_paths(nav_lib_path_env);
+    std::filesystem::path full_path = std::filesystem::path(build_dir) / lib_path_with_ext;
-    std::stringstream ss(nav_lib_paths);
+    if (std::filesystem::exists(full_path)) {
-    std::string base_dir;
+      try {
-    while (std::getline(ss, base_dir, ':')) {
+        return std::filesystem::canonical(full_path).string();
-      if (base_dir.empty()) {
+      } catch (...) {
-        continue;
+        return full_path.string();
-      }
-      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();
-        }
       }
     }
   }

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

@@ -141,13 +141,6 @@ namespace robot_nav_core
      */ 
     virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
 
-
-    /**
-     * @brief Gets the current global plan
-     * @param path The global plan
-     */
-    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
-
     /**
      * @brief  Constructs the local planner
      * @param name The name to give this instance of the local planner

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h

@@ -99,12 +99,6 @@ namespace robot_nav_core2
      */ 
     virtual void getPlan(robot_nav_2d_msgs::Path2D &path) = 0;
 
-    /**
-     * @brief Gets the current global plan
-     * @param path The global plan
-     */
-    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) = 0;
-
     /**
      * @brief Compute the best command given the current pose, velocity and goal
      *

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

@@ -167,12 +167,6 @@ namespace robot_nav_core_adapter
      */ 
     virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
 
-    /**
-     * @brief Gets the current global plan
-     * @param path The global plan
-     */
-    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
-
     /**
      * @brief Create a new LocalPlannerAdapter
      * @return A shared pointer to the new LocalPlannerAdapter

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

@@ -407,17 +407,6 @@ namespace robot_nav_core_adapter
     path = robot_nav_2d_utils::pathToPath(path2d).poses;
   }
 
-  void LocalPlannerAdapter::getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
-  {
-    if (!planner_)
-    {
-      return;
-    }
-    robot_nav_2d_msgs::Path2D path2d;
-    planner_->getGlobalPlan(path2d);
-    path = robot_nav_2d_utils::pathToPath(path2d).poses;
-  }
-
   bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
   {
     if (last_goal_.header.frame_id != new_goal.header.frame_id ||

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

@@ -271,7 +271,6 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(global_planner);
-      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, global_planner, boost::dll::load_mode::append_decorations);
 
@@ -315,7 +314,6 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(local_planner);
-      robot::log_info("[%s:%d]\n INFO: local_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       controller_loader_ =
           boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
               path_file_so, local_planner, boost::dll::load_mode::append_decorations);
@@ -403,7 +401,7 @@ void move_base::MoveBase::swapPlanner(std::string base_global_planner)
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(base_global_planner);
-      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
+
       auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
       auto new_planner = new_loader();
@@ -2007,7 +2005,6 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
         std::string behavior_name = behavior["name"].as<std::string>();
         robot::PluginLoaderHelper loader;
         std::string path_file_so = loader.findLibraryPath(behavior_type);
-        robot::log_info("Loading recovery behavior '%s' of type '%s' from '%s'", behavior_name.c_str(), behavior_type.c_str(), path_file_so.c_str());
         // Load the factory function from the shared library
         recovery_loaders_[behavior_name] = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
             path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
@@ -2867,7 +2864,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       {
         robot::log_debug("Goal reached!");
         resetState();
-        // swapPlanner(default_config_.base_global_planner);
+        swapPlanner(default_config_.base_global_planner);
         // disable the planner thread
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         runPlanner_ = false;
@@ -3073,7 +3070,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     {
       robot::log_debug("All recovery behaviors have failed, locking the planner and disabling it.");
       resetState();
-      // swapPlanner(default_config_.base_global_planner);
+      swapPlanner(default_config_.base_global_planner);
 
       // disable the planner thread
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -3214,22 +3211,6 @@ robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback()
 
 robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData()
 {
-  if (tc_)
-  {
-    robot_nav_msgs::Path path;
-    tc_->getGlobalPlan(path.poses);
-    if (!path.poses.empty())
-    {
-      path.header.stamp = path.poses[0].header.stamp;
-      path.header.frame_id = path.poses[0].header.frame_id;
-    }
-    else
-    {
-      path.header.stamp = robot::Time::now();
-      path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
-    }
-    global_data_.plan = robot_nav_2d_utils::pathToPath(path);
-  }
   ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true);
   convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);
   convert_data.updateFootprint(global_data_.footprint);