From 6fbbbc0bb8b9811e87c54a35a8312316acdb019a Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Wed, 26 Jun 2024 14:47:29 +1000 Subject: [PATCH] Add error_msg handling to nav2_bt_navigators (#4341) Signed-off-by: Mike Wake --- .../navigators/navigate_through_poses.hpp | 1 + .../navigators/navigate_to_pose.hpp | 1 + .../src/navigators/navigate_through_poses.cpp | 49 ++++++++++++----- .../src/navigators/navigate_to_pose.cpp | 54 ++++++++++++++----- .../nav2_core/behavior_tree_navigator.hpp | 7 +++ .../nav2_core/navigator_exceptions.hpp | 47 ++++++++++++++++ nav2_msgs/action/NavigateThroughPoses.action | 5 ++ nav2_msgs/action/NavigateToPose.action | 5 ++ 8 files changed, 142 insertions(+), 27 deletions(-) create mode 100644 nav2_core/include/nav2_core/navigator_exceptions.hpp diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index aee07de14c9..86e852ebc9e 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -22,6 +22,7 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/behavior_tree_navigator.hpp" +#include "nav2_core/navigator_exceptions.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 447d02cc449..73c5d59377b 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -22,6 +22,7 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/behavior_tree_navigator.hpp" +#include "nav2_core/navigator_exceptions.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 98d86499800..c444b4a4caf 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -18,6 +18,7 @@ #include #include #include "nav2_bt_navigator/navigators/navigate_through_poses.hpp" +#include "nav2_core/navigator_exceptions.hpp" namespace nav2_bt_navigator { @@ -45,6 +46,8 @@ NavigateThroughPosesNavigator::configure( // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; + current_error_code_ = ActionT::Result::NONE; + current_error_msg_ = ""; return true; } @@ -75,9 +78,10 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) auto bt_xml_filename = goal->behavior_tree; if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { - RCLCPP_ERROR( - logger_, "Error loading XML file: %s. Navigation canceled.", - bt_xml_filename.c_str()); + current_error_code_ = ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE; + current_error_msg_ = "Error loading XML file: " + bt_xml_filename + + ". Navigation canceled."; + RCLCPP_ERROR(logger_, current_error_msg_.c_str()); return false; } @@ -86,9 +90,11 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) void NavigateThroughPosesNavigator::goalCompleted( - typename ActionT::Result::SharedPtr /*result*/, + typename ActionT::Result::SharedPtr result, const nav2_behavior_tree::BtStatus /*final_bt_status*/) { + result->error_code = current_error_code_; + result->error_msg = current_error_msg_; } void @@ -116,8 +122,7 @@ NavigateThroughPosesNavigator::onLoop() feedback_utils_.global_frame, feedback_utils_.robot_frame, feedback_utils_.transform_tolerance)) { - RCLCPP_ERROR(logger_, "Robot pose is not available."); - return; + throw nav2_core::NavigatorPoseNotAvailable("Robot pose is not available."); } try { @@ -125,7 +130,7 @@ NavigateThroughPosesNavigator::onLoop() nav_msgs::msg::Path current_path; if (!blackboard->get(path_blackboard_id_, current_path) || current_path.poses.size() == 0u) { // If no path set yet or not meaningful, can't compute ETA or dist remaining yet. - throw std::exception(); + throw nav2_core::NavigatorInvalidPath("no valid path available"); } // Find the closest pose to current pose on global path @@ -164,8 +169,21 @@ NavigateThroughPosesNavigator::onLoop() feedback_msg->distance_remaining = distance_remaining; feedback_msg->estimated_time_remaining = estimated_time_remaining; - } catch (...) { - // Ignore + } catch (const nav2_core::NavigatorPoseNotAvailable & ex) { + current_error_code_ = ActionT::Result::POSE_NOT_AVAILABLE; + current_error_msg_ = ex.what(); + RCLCPP_ERROR(logger_, current_error_msg_.c_str()); + // Returning since no point attempting recovery or publishing + // feedback until robot pose is available. + return; + } catch (const nav2_core::NavigatorInvalidPath & ex) { + current_error_code_ = ActionT::Result::NO_VALID_PATH; + current_error_msg_ = ex.what(); + // Ignore ?? + } catch (const std::runtime_error & ex) { + current_error_code_ = ActionT::Result::UNKNOWN; + current_error_msg_ = ex.what(); + // Ignore ?? } int recovery_count = 0; @@ -218,10 +236,15 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.transform_tolerance)) { - RCLCPP_ERROR( - logger_, - "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.", - goal_pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str()); + current_error_code_ = ActionT::Result::TF_ERROR; + current_error_msg_ = + "Failed to transform a goal pose provided with frame_id '" + + goal_pose.header.frame_id + + "' to the global frame '" + + feedback_utils_.global_frame + + "'."; + + RCLCPP_ERROR(logger_, current_error_msg_.c_str()); return false; } } diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index be6a2948358..2ee89cece21 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -17,6 +17,7 @@ #include #include #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp" +#include "nav2_core/navigator_exceptions.hpp" namespace nav2_bt_navigator { @@ -44,6 +45,9 @@ NavigateToPoseNavigator::configure( // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; + current_error_code_ = ActionT::Result::NONE; + current_error_msg_ = ""; + self_client_ = rclcpp_action::create_client(node, getName()); goal_sub_ = node->create_subscription( @@ -88,9 +92,10 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) auto bt_xml_filename = goal->behavior_tree; if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { - RCLCPP_ERROR( - logger_, "BT file not found: %s. Navigation canceled.", - bt_xml_filename.c_str()); + current_error_code_ = ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE; + current_error_msg_ = "Error loading XML file: " + bt_xml_filename + + ". Navigation canceled."; + RCLCPP_ERROR(logger_, current_error_msg_.c_str()); return false; } @@ -99,9 +104,11 @@ 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*/) { + result->error_code = current_error_code_; + result->error_msg = current_error_msg_; } void @@ -117,8 +124,7 @@ NavigateToPoseNavigator::onLoop() feedback_utils_.global_frame, feedback_utils_.robot_frame, feedback_utils_.transform_tolerance)) { - RCLCPP_ERROR(logger_, "Robot pose is not available."); - return; + throw nav2_core::NavigatorPoseNotAvailable("Robot pose is not available."); } auto blackboard = bt_action_server_->getBlackboard(); @@ -128,7 +134,7 @@ NavigateToPoseNavigator::onLoop() nav_msgs::msg::Path current_path; if (!blackboard->get(path_blackboard_id_, current_path) || current_path.poses.size() == 0u) { // If no path set yet or not meaningful, can't compute ETA or dist remaining yet. - throw std::exception(); + throw nav2_core::NavigatorInvalidPath("no valid path available"); } // Find the closest pose to current pose on global path @@ -167,8 +173,21 @@ NavigateToPoseNavigator::onLoop() feedback_msg->distance_remaining = distance_remaining; feedback_msg->estimated_time_remaining = estimated_time_remaining; - } catch (...) { - // Ignore + } catch (const nav2_core::NavigatorPoseNotAvailable & ex) { + current_error_code_ = ActionT::Result::POSE_NOT_AVAILABLE; + current_error_msg_ = ex.what(); + RCLCPP_ERROR(logger_, current_error_msg_.c_str()); + // Returning since no point attempting recovery or publishing + // feedback until robot pose is available. + return; + } catch (const nav2_core::NavigatorInvalidPath & ex) { + current_error_code_ = ActionT::Result::NO_VALID_PATH; + current_error_msg_ = ex.what(); + // Ignore ?? + } catch (const std::runtime_error & ex) { + current_error_code_ = ActionT::Result::UNKNOWN; + current_error_msg_ = ex.what(); + // Ignore ?? } int recovery_count = 0; @@ -220,7 +239,9 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) feedback_utils_.global_frame, feedback_utils_.robot_frame, feedback_utils_.transform_tolerance)) { - RCLCPP_ERROR(logger_, "Initial robot pose is not available."); + current_error_code_ = ActionT::Result::POSE_NOT_AVAILABLE; + current_error_msg_ = "Initial robot pose is not available."; + RCLCPP_ERROR(logger_, current_error_msg_.c_str()); return false; } @@ -229,10 +250,15 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.transform_tolerance)) { - RCLCPP_ERROR( - logger_, - "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.", - goal->pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str()); + current_error_code_ = ActionT::Result::TF_ERROR; + current_error_msg_ = + "Failed to transform a goal pose provided with frame_id '" + + goal->pose.header.frame_id + + "' to the global frame '" + + feedback_utils_.global_frame + + "'."; + + RCLCPP_ERROR(logger_, current_error_msg_.c_str()); return false; } diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index f6b8ecd62a4..e5392488c32 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -225,6 +225,9 @@ class BehaviorTreeNavigator : public NavigatorBase blackboard->set("number_recoveries", 0); // NOLINT blackboard->set("odom_smoother", odom_smoother); // NOLINT + current_error_code_ = 0; + current_error_msg_ = ""; + return configure(parent_node, odom_smoother) && ok; } @@ -371,6 +374,10 @@ class BehaviorTreeNavigator : public NavigatorBase rclcpp::Clock::SharedPtr clock_; FeedbackUtils feedback_utils_; NavigatorMuxer * plugin_muxer_; + + // Error tracking + uint16_t current_error_code_; + std::string current_error_msg_; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/navigator_exceptions.hpp b/nav2_core/include/nav2_core/navigator_exceptions.hpp new file mode 100644 index 00000000000..53565a0df30 --- /dev/null +++ b/nav2_core/include/nav2_core/navigator_exceptions.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_CORE__NAVIGATOR_EXCEPTIONS_HPP_ +#define NAV2_CORE__NAVIGATOR_EXCEPTIONS_HPP_ + +#include +#include + +namespace nav2_core +{ + +class NavigatorException : public std::runtime_error +{ +public: + explicit NavigatorException(const std::string & description) + : std::runtime_error(description) {} +}; + +class NavigatorPoseNotAvailable : public NavigatorException +{ +public: + explicit NavigatorPoseNotAvailable(const std::string & description) + : NavigatorException(description) {} +}; + +class NavigatorInvalidPath : public NavigatorException +{ +public: + explicit NavigatorInvalidPath(const std::string & description) + : NavigatorException(description) {} +}; + +} // namespace nav2_core + +#endif // NAV2_CORE__NAVIGATOR_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 898e4540ef7..6708778cc1e 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -8,6 +8,11 @@ string behavior_tree # Error codes # Note: The expected priority order of the errors should match the message order uint16 NONE=0 +uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=901 +uint16 TF_ERROR=902 +uint16 POSE_NOT_AVAILABLE=912 +uint16 NO_VALID_PATH=913 +uint16 UNKNOWN=999 uint16 error_code string error_msg diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index daec2756621..7afea7c8e22 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -7,6 +7,11 @@ string behavior_tree # Error codes # Note: The expected priority order of the errors should match the message order uint16 NONE=0 +uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=901 +uint16 TF_ERROR=902 +uint16 POSE_NOT_AVAILABLE=912 +uint16 NO_VALID_PATH=913 +uint16 UNKNOWN=999 uint16 error_code string error_msg