add docking to

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/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
+
+MKTDockingPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+MKTGoStraightPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+MKTRotatePlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
 
 ### replanning
 controller_frequency: 30.0   # run controller at 15.0 Hz

examples/NavigationExample/NavigationAPI.cs

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

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

@@ -397,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
   {