diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index f606ea9..997b55c 100755 --- a/config/costmap_common_params.yaml +++ b/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 diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index 73110da..72838cc 100755 --- a/config/move_base_common_params.yaml +++ b/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 diff --git a/examples/NavigationExample/NavigationAPI.cs b/examples/NavigationExample/NavigationAPI.cs index fd411fa..146fbf1 100644 --- a/examples/NavigationExample/NavigationAPI.cs +++ b/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); diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index c508ffe..302b419 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/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 diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index fcafddf..78a7f01 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/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 nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) + return false; + Order order; + order.nodes = const_cast(nodes); + order.nodes_count = node_count; + order.edges = const_cast(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) diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index aef61aa..b67434c 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/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 {