add dock to

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

@@ -337,38 +337,29 @@ extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const No
 }
 extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal)
 {
-  robot::log_error("navigation_dock_to marker %s, goal %f %f", marker, goal.pose.position.x, goal.pose.position.y);
   if (!handle || !marker)
   {
-    robot::log_error("Invalid parameters: handle or marker is null");
     return false;
   }
-  robot::log_error("debug 1");
   if (!isQuaternionValid(goal.pose.orientation))
   {
     robot::log_error("Goal quaternion is invalid");
     return false;
   }
-  robot::log_error("debug 2");
+
   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 *) {});
-    robot::log_error("debug 3");
     if (!nav_ptr)
-    {
-      robot::log_error("Failed to retrieve navigation instance");
       return false;
-    }
+
-    robot::log_error("debug 4");
     robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
-    return nav_ptr->dockTo(std::string(marker), cpp_goal);
+  return nav_ptr->dockTo(std::string(marker), cpp_goal);
-    robot::log_error("debug 5");
   }
   catch (...)
   {
-    robot::log_error("Failed to dock to marker");
     return false;
   }
 }
@@ -406,6 +397,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/Navigations/Packages/move_base/src/move_base.cpp

@@ -812,7 +812,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
   {
@@ -963,7 +973,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
   {
@@ -1120,7 +1140,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
   {
@@ -1233,7 +1263,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
   {
@@ -1342,7 +1382,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
   {
@@ -1465,7 +1515,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
   {