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