From 7097deea781e4a1322d145f3fa0505a39f909f0a Mon Sep 17 00:00:00 2001 From: panav Date: Wed, 8 Oct 2025 21:42:36 +0530 Subject: [PATCH] Added error feedback for nav go to --- .../src/navigators/navigate_to_pose.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index bc22714ef58..5bfc04c5495 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -102,9 +102,21 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) void NavigateToPoseNavigator::goalCompleted( - typename ActionT::Result::SharedPtr /*result*/, + typename ActionT::Result::SharedPtr result, const nav2_behavior_tree::BtStatus /*final_bt_status*/) { + if (result->error_code == 0) { + if (bt_action_server_->populateInternalError(result)) { + RCLCPP_WARN(logger_, + "NavigateToPoseNavigator::goalCompleted, internal error %d:%s.", + result->error_code, + result->error_msg.c_str()); + } + } else { + RCLCPP_WARN(logger_, "NavigateToPoseNavigator::goalCompleted error %d:%s.", + result->error_code, + result->error_msg.c_str()); + } } void