update plan when docking

examples/NavigationExample/NavigationExample.csproj

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

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

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

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

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

@@ -52,6 +52,12 @@ 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,7 +254,8 @@ 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);
 
@@ -326,8 +327,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_);
 
-      planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
-      planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+      parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
 
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
       {
@@ -560,7 +561,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
         {
           if(!dkpl_.empty()) 
           {
-            if(dkpl_.front()) delete(dkpl_.front());
+            // if(dkpl_.front()) delete(dkpl_.front());
             dkpl_.erase(dkpl_.begin());
           }
           start_docking_ = false;

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

@@ -235,6 +235,15 @@ 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 (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

@@ -16,6 +16,70 @@
 #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
 {
 
@@ -1147,92 +1211,14 @@ 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
     {
-      switch (v.getType())
-      {
-      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;
-      }
+      robot_xmlrpcpp::XmlRpcValue v_copy = v;
+      YAML::Node node = xmlRpcToYaml(v_copy);
+      const_cast<NodeHandle *>(this)->setParamInternal(key, node, yamlNodeCategory(node));
     }
     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,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) {
@@ -199,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)
-  std::string build_dir = getBuildDirectory();
-  if (!build_dir.empty()) {
-    // First try in build directory
-    // Add .so extension if not present
+  // If relative path, search under PNKX_NAV_CORE_LIBRARY_PATH (':'-separated, like LD_LIBRARY_PATH)
+  const char* nav_lib_path_env = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
+  if (nav_lib_path_env) {
     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;
-    if (std::filesystem::exists(full_path)) {
-      try {
-        return std::filesystem::canonical(full_path).string();
-      } catch (...) {
-        return full_path.string();
+    std::string nav_lib_paths(nav_lib_path_env);
+    std::stringstream ss(nav_lib_paths);
+    std::string base_dir;
+    while (std::getline(ss, base_dir, ':')) {
+      if (base_dir.empty()) {
+        continue;
+      }
+      std::filesystem::path full_path = std::filesystem::path(base_dir) / lib_path_with_ext;
+      if (std::filesystem::exists(full_path)) {
+        try {
+          return std::filesystem::canonical(full_path).string();
+        } catch (...) {
+          return full_path.string();
+        }
       }
     }
   }

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

@@ -141,6 +141,13 @@ 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,6 +99,12 @@ 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,6 +167,12 @@ 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,6 +407,17 @@ 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,6 +271,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(global_planner);
+      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, global_planner, boost::dll::load_mode::append_decorations);
 
@@ -314,6 +315,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(local_planner);
+      robot::log_info("[%s:%d]\n INFO: local_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       controller_loader_ =
           boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
               path_file_so, local_planner, boost::dll::load_mode::append_decorations);
@@ -401,7 +403,7 @@ void move_base::MoveBase::swapPlanner(std::string base_global_planner)
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(base_global_planner);
-
+      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
       auto new_planner = new_loader();
@@ -2005,6 +2007,7 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
         std::string behavior_name = behavior["name"].as<std::string>();
         robot::PluginLoaderHelper loader;
         std::string path_file_so = loader.findLibraryPath(behavior_type);
+        robot::log_info("Loading recovery behavior '%s' of type '%s' from '%s'", behavior_name.c_str(), behavior_type.c_str(), path_file_so.c_str());
         // Load the factory function from the shared library
         recovery_loaders_[behavior_name] = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
             path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
@@ -2864,7 +2867,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       {
         robot::log_debug("Goal reached!");
         resetState();
-        swapPlanner(default_config_.base_global_planner);
+        // swapPlanner(default_config_.base_global_planner);
         // disable the planner thread
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         runPlanner_ = false;
@@ -3070,7 +3073,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     {
       robot::log_debug("All recovery behaviors have failed, locking the planner and disabling it.");
       resetState();
-      swapPlanner(default_config_.base_global_planner);
+      // swapPlanner(default_config_.base_global_planner);
 
       // disable the planner thread
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -3211,6 +3214,22 @@ 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);