Add error_msg handling to nav2_bt_navigators (#4341)

Signed-off-by: Mike Wake <[email protected]>

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"

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"

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -18,6 +18,7 @@
 #include <memory>
 #include <limits>
 #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,16 +122,15 @@ 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 {
     // Get current path points
     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;
     }
   }

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -17,6 +17,7 @@
 #include <memory>
 #include <limits>
 #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<ActionT>(node, getName());
 
   goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
@@ -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;
   }
 

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

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 <stdexcept>
+#include <string>
+
+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_

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

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