HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add dock to

Browse Source
This commit is contained in:
HiepLM
2026-03-19 04:25:59 +00:00
parent e5c04f476b
commit 56ccd202d0
3 changed files with 121 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/APIs/c_api/include/nav_c_api.h
View File

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

51
src/APIs/c_api/src/nav_c_api.cpp
View File

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

60
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -811,9 +811,19 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
if (setup_)
{
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
{
if (nav_feedback_)
@@ -962,9 +972,19 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
if (setup_)
{
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
{
if (nav_feedback_)
@@ -1119,9 +1139,19 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
}
if (setup_)
{
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
{
if (nav_feedback_)
@@ -1232,9 +1262,19 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
}
if (setup_)
{
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
{
if (nav_feedback_)
@@ -1342,8 +1382,18 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
if (setup_)
{
std::string global_planner = "TwoPointsPlanner";
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
{
if (nav_feedback_)
@@ -1465,8 +1515,18 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
if (setup_)
{
std::string global_planner = "TwoPointsPlanner";
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
{
if (nav_feedback_)