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

add log

Browse Source
This commit is contained in:
HiepLM
2026-03-19 04:12:51 +00:00
parent f02c20cc5c
commit e5c04f476b
1 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -337,29 +337,38 @@ 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) 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) if (!handle || !marker)
{ {
robot::log_error("Invalid parameters: handle or marker is null");
return false; return false;
} }
robot::log_error("debug 1");
if (!isQuaternionValid(goal.pose.orientation)) if (!isQuaternionValid(goal.pose.orientation))
{ {
robot::log_error("Goal quaternion is invalid"); robot::log_error("Goal quaternion is invalid");
return false; return false;
} }
robot::log_error("debug 2");
try try
{ {
std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr = std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
std::shared_ptr<robot::move_base_core::BaseNavigation>( std::shared_ptr<robot::move_base_core::BaseNavigation>(
reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::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) if (!nav_ptr)
{
robot::log_error("Failed to retrieve navigation instance");
return false; return false;
}
robot::log_error("debug 4");
robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); 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 (...) catch (...)
{ {
robot::log_error("Failed to dock to marker");
return false; return false;
} }
} }