From e5c04f476bf953b54820171c6c77a95f8828dfdb Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 19 Mar 2026 04:12:51 +0000 Subject: [PATCH] add log --- src/APIs/c_api/src/nav_c_api.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index fcafddf..36843e4 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -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) { + 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); + 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; } }