From 56ccd202d0b045edcdddcb6532a867835c16827d Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 19 Mar 2026 04:25:59 +0000 Subject: [PATCH] add dock to --- src/APIs/c_api/include/nav_c_api.h | 14 ++++ src/APIs/c_api/src/nav_c_api.cpp | 53 ++++++++++---- .../Packages/move_base/src/move_base.cpp | 72 +++++++++++++++++-- 3 files changed, 121 insertions(+), 18 deletions(-) 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 36843e4..78a7f01 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/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 nav_ptr = std::shared_ptr( reinterpret_cast(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); - robot::log_error("debug 5"); + return nav_ptr->dockTo(std::string(marker), cpp_goal); } 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 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 {