Pipe error codes (ros-navigation#3251)

* issue with finding key

* passed up codes to bt_navigator

* lint fix

* updates

* adding error_code_id back in

* error codes names in params

* bump error codes

* lint

* spelling

* test fix

* update behavior trees

* cleanup

* Update bt_action_server_impl.hpp

* code review

* lint

* code review

* log fix

* error code for waypoint follower

* clean up

* remove waypoint error test, too flaky on CI

* lint and code review

* rough imp for waypoint changes

* lint

* code review

* build fix

* clean up

* revert

* space

* remove

* try to make github happ

* stop gap

* loading in param file

* working tests :)

* lint

* fixed cmake

* lint

* lint

* trigger build

* added invalid plugin error

* added test for piping up error codes

* clean up

* test waypoint follower

* only launch what is needed

* waypoint test

* revert lines for robot navigator

* fix test

* waypoint test

* switched to uint16

* clean up

* code review

* todo to note

* lint

* remove comment

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* rename error_codes

* error code for navigate to pose

* error codes for navigate through poses.

* error codes for navigate through poses

* message update for waypoint follower

* rename to error code

* update node xml

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

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>
@@ -101,6 +120,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_);
 
@@ -223,6 +245,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) {
@@ -243,6 +268,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>(
+          "error_code_id", "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>(
+          "error_code_id", "Navigate to pose error code"),
       });
   }
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -77,7 +77,7 @@
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
-      <output_port name="compute_path_to_pose_error_code">"The compute path to pose error code"</output_port>
+      <output_port name="error_code_id">"Compute path to pose error code"</output_port>
     </Action>
 
     <Action ID="ComputePathThroughPoses">
@@ -87,8 +87,7 @@
       <input_port name="server_timeout">Server timeout</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
-      <output_port name="compute_path_through_poses_error_code">"The compute path through poses
-        pose error code"</output_port>
+      <output_port name="error_code_id">"Compute path through poses error code"</output_port>
     </Action>
 
     <Action ID="RemovePassedGoals">
@@ -115,21 +114,23 @@
       <input_port name="goal_checker_id">Goal checker</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
-      <output_port name="follow_path_error_code">Follow Path error code</output_port>
+      <output_port name="error_code_id">Follow Path error code</output_port>
     </Action>
 
     <Action ID="NavigateToPose">
         <input_port name="goal">Goal</input_port>
         <input_port name="server_name">Server name</input_port>
         <input_port name="server_timeout">Server timeout</input_port>
         <input_port name="behavior_tree">Behavior tree to run</input_port>
+        <output_port name="error_code_id">Navigate to pose error code</output_port>
     </Action>
 
     <Action ID="NavigateThroughPoses">
         <input_port name="goals">Goals</input_port>
         <input_port name="server_name">Server name</input_port>
         <input_port name="server_timeout">Server timeout</input_port>
         <input_port name="behavior_tree">Behavior tree to run</input_port>
+        <output_port name="error_code_id">Navigate through poses error code</output_port>
     </Action>
 
     <Action ID="ReinitializeGlobalLocalization">

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("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus NavigateThroughPosesAction::on_aborted()
+{
+  setOutput("error_code_id", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus NavigateThroughPosesAction::on_cancelled()
+{
+  // Set empty error code, action was cancelled
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"