Pipe error codes

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -189,6 +189,13 @@ class BtActionServer
    */
   void executeCallback();
 
+  /**
+   * @brief updates the action server result to the highest priority error code posted on the
+   * blackboard
+   * @param result the action server result to be updated
+   */
+  void populateErrorCode(typename std::shared_ptr<typename ActionT::Result> result);
+
   // Action name
   std::string action_name_;
 
@@ -211,6 +218,9 @@ class BtActionServer
   // Libraries to pull plugins (BT Nodes) from
   std::vector<std::string> plugin_lib_names_;
 
+  // Error code id names
+  std::vector<std::string> error_code_names_;
+
   // A regular, non-spinning ROS node that we can use for calls to the action client
   rclcpp::Node::SharedPtr client_node_;
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -21,6 +21,7 @@
 #include <set>
 #include <exception>
 #include <vector>
+#include <limits>
 
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_behavior_tree/bt_action_server.hpp"
@@ -59,6 +60,24 @@ BtActionServer<ActionT>::BtActionServer(
   if (!node->has_parameter("default_server_timeout")) {
     node->declare_parameter("default_server_timeout", 20);
   }
+
+  std::vector<std::string> error_code_names = {
+    "follow_path_error_code",
+    "compute_path_error_code"
+  };
+
+  if (!node->has_parameter("error_code_names")) {
+    std::string error_codes_str;
+    for (const auto & error_code : error_code_names) {
+      error_codes_str += error_code + "\n";
+    }
+    RCLCPP_WARN_STREAM(
+      logger_, "Error_code parameters were not set. Using default values of: "
+        << error_codes_str
+        << "Make sure these match your BT and there are not other sources of error codes you want "
+        "reported to your application");
+    node->declare_parameter("error_code_names", error_code_names);
+  }
 }
 
 template<class ActionT>
@@ -104,6 +123,9 @@ bool BtActionServer<ActionT>::on_configure()
   node->get_parameter("default_server_timeout", timeout);
   default_server_timeout_ = std::chrono::milliseconds(timeout);
 
+  // Get error code id names to grab off of the blackboard
+  error_code_names_ = node->get_parameter("error_code_names").as_string_array();
+
   // Create the class that registers our custom nodes and executes the BT
   bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
 
@@ -226,6 +248,9 @@ void BtActionServer<ActionT>::executeCallback()
   // Give server an opportunity to populate the result message or simple give
   // an indication that the action is complete.
   auto result = std::make_shared<typename ActionT::Result>();
+
+  populateErrorCode(result);
+
   on_completion_callback_(result, rc);
 
   switch (rc) {
@@ -246,6 +271,30 @@ void BtActionServer<ActionT>::executeCallback()
   }
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::populateErrorCode(
+  typename std::shared_ptr<typename ActionT::Result> result)
+{
+  int highest_priority_error_code = std::numeric_limits<int>::max();
+  for (const auto & error_code : error_code_names_) {
+    try {
+      int current_error_code = blackboard_->get<int>(error_code);
+      if (current_error_code != 0 && current_error_code < highest_priority_error_code) {
+        highest_priority_error_code = current_error_code;
+      }
+    } catch (...) {
+      RCLCPP_ERROR(
+        logger_,
+        "Failed to get error code: %s from blackboard",
+        error_code.c_str());
+    }
+  }
+
+  if (highest_priority_error_code != std::numeric_limits<int>::max()) {
+    result->error_code = highest_priority_error_code;
+  }
+}
+
 }  // namespace nav2_behavior_tree
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp

@@ -86,7 +86,7 @@ class ComputePathThroughPosesAction
           "Mapped name to the planner plugin type to use"),
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
         BT::OutputPort<ActionResult::_error_code_type>(
-          "compute_path_through_poses_error_code", "The compute path through poses error code"),
+          "error_code_id", "The compute path through poses error code"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

@@ -86,7 +86,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
           "Mapped name to the planner plugin type to use"),
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
         BT::OutputPort<ActionResult::_error_code_type>(
-          "compute_path_to_pose_error_code", "The compute path to pose error code"),
+          "error_code_id", "The compute path to pose error code"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -85,7 +85,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
         BT::InputPort<std::string>("controller_id", ""),
         BT::InputPort<std::string>("goal_checker_id", ""),
         BT::OutputPort<ActionResult::_error_code_type>(
-          "follow_path_error_code", "The follow path error code"),
+          "error_code_id", "The follow path error code"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp

@@ -30,6 +30,10 @@ namespace nav2_behavior_tree
  */
 class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::NavigateThroughPoses>
 {
+  using Action = nav2_msgs::action::NavigateThroughPoses;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::NavigateThroughPosesAction
@@ -47,6 +51,21 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
    */
   void on_tick() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon successful completion of the action
+   */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -57,6 +76,8 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goals", "Destinations to plan through"),
         BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+            "navigate_through_poses_error_code", "The navigate through poses error code"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp

@@ -30,6 +30,10 @@ namespace nav2_behavior_tree
  */
 class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
 {
+  using Action = nav2_msgs::action::NavigateToPose;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::NavigateToPoseAction
@@ -47,6 +51,21 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
    */
   void on_tick() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon successful completion of the action
+   */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -57,6 +76,8 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
         BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+            "navigate_to_pose_error_code", "Navigate to pose error code"),
       });
   }
 };

nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp

@@ -42,15 +42,15 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success()
 {
   setOutput("path", result_.result->path);
   // Set empty error code, action was successful
-  setOutput("compute_path_through_poses_error_code", ActionGoal::NONE);
+  setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 
 BT::NodeStatus ComputePathThroughPosesAction::on_aborted()
 {
   nav_msgs::msg::Path empty_path;
   setOutput("path", empty_path);
-  setOutput("compute_path_through_poses_error_code", result_.result->error_code);
+  setOutput("error_code_id", result_.result->error_code);
   return BT::NodeStatus::FAILURE;
 }
 
@@ -59,7 +59,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
   nav_msgs::msg::Path empty_path;
   setOutput("path", empty_path);
   // Set empty error code, action was cancelled
-  setOutput("compute_path_through_poses_error_code", ActionGoal::NONE);
+  setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -41,15 +41,15 @@ BT::NodeStatus ComputePathToPoseAction::on_success()
 {
   setOutput("path", result_.result->path);
   // Set empty error code, action was successful
-  setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
+  setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 
 BT::NodeStatus ComputePathToPoseAction::on_aborted()
 {
   nav_msgs::msg::Path empty_path;
   setOutput("path", empty_path);
-  setOutput("compute_path_to_pose_error_code", result_.result->error_code);
+  setOutput("error_code_id", result_.result->error_code);
   return BT::NodeStatus::FAILURE;
 }
 
@@ -58,7 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled()
   nav_msgs::msg::Path empty_path;
   setOutput("path", empty_path);
   // Set empty error code, action was cancelled
-  setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
+  setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -37,20 +37,20 @@ void FollowPathAction::on_tick()
 
 BT::NodeStatus FollowPathAction::on_success()
 {
-  setOutput("follow_path_error_code", ActionGoal::NONE);
+  setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 
 BT::NodeStatus FollowPathAction::on_aborted()
 {
-  setOutput("follow_path_error_code", result_.result->error_code);
+  setOutput("error_code_id", result_.result->error_code);
   return BT::NodeStatus::FAILURE;
 }
 
 BT::NodeStatus FollowPathAction::on_cancelled()
 {
   // Set empty error code, action was cancelled
-  setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
+  setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp

@@ -39,6 +39,26 @@ void NavigateThroughPosesAction::on_tick()
   getInput("behavior_tree", goal_.behavior_tree);
 }
 
+BT::NodeStatus NavigateThroughPosesAction::on_success()
+{
+  setOutput("navigate_through_poses_error_code", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus NavigateThroughPosesAction::on_aborted()
+{
+  setOutput("navigate_through_poses_error_code", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus NavigateThroughPosesAction::on_cancelled()
+{
+  // Set empty error code, action was cancelled
+  setOutput("navigate_through_poses_error_code", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp

@@ -24,9 +24,8 @@ NavigateToPoseAction::NavigateToPoseAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::NavigateToPose>(xml_tag_name, action_name, conf)
-{
-}
+: BtActionNode<Action>(xml_tag_name, action_name, conf)
+{}
 
 void NavigateToPoseAction::on_tick()
 {
@@ -39,6 +38,25 @@ void NavigateToPoseAction::on_tick()
   getInput("behavior_tree", goal_.behavior_tree);
 }
 
+BT::NodeStatus NavigateToPoseAction::on_success()
+{
+  setOutput("navigate_to_pose_error_code", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus NavigateToPoseAction::on_aborted()
+{
+  setOutput("navigate_to_pose_error_code", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus NavigateToPoseAction::on_cancelled()
+{
+  // Set empty error code, action was cancelled
+  setOutput("navigate_to_pose_error_code", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"