Behavior server error codes

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp

@@ -29,6 +29,10 @@ namespace nav2_behavior_tree
  */
 class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTeleop>
 {
+  using Action = nav2_msgs::action::AssistedTeleop;
+  using ActionGoal = Action::Goal;
+  using ActionResult = Action::Result;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::nav2_msgs::action::AssistedTeleop
@@ -46,8 +50,21 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
    */
   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,7 +74,9 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
     return providedBasicPorts(
       {
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for running assisted teleop"),
-        BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented")
+        BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "The assisted teleop behavior server error code")
       });
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -28,6 +28,10 @@ namespace nav2_behavior_tree
  */
 class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
 {
+  using Action = nav2_msgs::action::BackUp;
+  using ActionGoal = Action::Goal;
+  using ActionResult = Action::Result;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::BackUpAction
@@ -45,6 +49,22 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
    */
   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
@@ -55,7 +75,9 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
       {
         BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "The back up behavior server error code")
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

@@ -28,6 +28,10 @@ namespace nav2_behavior_tree
  */
 class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeading>
 {
+  using Action = nav2_msgs::action::DriveOnHeading;
+  using ActionGoal = Action::Goal;
+  using ActionResult = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::DriveOnHeadingAction
@@ -50,11 +54,27 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
       {
         BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"),
         BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
+        BT::OutputPort<Action::Result::_error_code_type>(
+          "error_code_id", "The drive on heading behavior server error code")
       });
   }
+
+  /**
+ * @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;
 };
 
 }  // namespace nav2_behavior_tree
-
 #endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

@@ -28,6 +28,10 @@ namespace nav2_behavior_tree
  */
 class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 {
+  using Action = nav2_msgs::action::Spin;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::SpinAction
@@ -55,10 +59,27 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
       {
         BT::InputPort<double>("spin_dist", 1.57, "Spin distance"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning"),
-        BT::InputPort<bool>("is_recovery", true, "True if recovery")
+        BT::InputPort<bool>("is_recovery", true, "True if recovery"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "The spin behavior error code")
       });
   }
 
+  /**
+   * @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;
+
 private:
   bool is_recovery_;
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -13,6 +13,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="error_code_id">"Back up error code"</output_port>
     </Action>
 
     <Action ID="DriveOnHeading">
@@ -21,6 +22,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="error_code_id">"Drive on heading error code"</output_port>
     </Action>
 
     <Action ID="CancelControl">
@@ -184,6 +186,7 @@
       <input_port name="time_allowance">Allowed time for spinning</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="error_code_id">Spin error code</output_port>
     </Action>
 
     <Action ID="Wait">
@@ -197,6 +200,7 @@
       <input_port name="is_recovery">If true recovery count will be incremented</input_port>
       <input_port name="server_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="error_code_id">Assisted teleop error code</output_port>
     </Action>
 
     <!-- ############################### CONDITION NODES ############################## -->

nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp

@@ -41,11 +41,24 @@ void AssistedTeleopAction::on_tick()
   }
 }
 
+BT::NodeStatus AssistedTeleopAction::on_success()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 BT::NodeStatus AssistedTeleopAction::on_aborted()
 {
+  setOutput("error_code_id", result_.result->error_code);
   return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus AssistedTeleopAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

nav2_behavior_tree/plugins/action/back_up_action.cpp

@@ -46,6 +46,24 @@ void BackUpAction::on_tick()
   increment_recovery_count();
 }
 
+BT::NodeStatus BackUpAction::on_success()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus BackUpAction::on_aborted()
+{
+  setOutput("error_code_id", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus BackUpAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp

@@ -41,6 +41,24 @@ DriveOnHeadingAction::DriveOnHeadingAction(
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
 }
 
+BT::NodeStatus DriveOnHeadingAction::on_success()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus DriveOnHeadingAction::on_aborted()
+{
+  setOutput("error_code_id", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus DriveOnHeadingAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

nav2_behavior_tree/plugins/action/spin_action.cpp

@@ -12,7 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <string>
 #include <memory>
 
 #include "nav2_behavior_tree/plugins/action/spin_action.hpp"
@@ -42,6 +41,24 @@ void SpinAction::on_tick()
   }
 }
 
+BT::NodeStatus SpinAction::on_success()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus SpinAction::on_aborted()
+{
+  setOutput("error_code_id", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus SpinAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -45,6 +45,12 @@ enum class Status : int8_t
   RUNNING = 3,
 };
 
+struct ResultStatus
+{
+  Status status;
+  unsigned short error_code{0};  //NOLINT
+};
+
 using namespace std::chrono_literals;  //NOLINT
 
 /**
@@ -73,15 +79,15 @@ class TimedBehavior : public nav2_core::Behavior
   // Derived classes can override this method to catch the command and perform some checks
   // before getting into the main loop. The method will only be called
   // once and should return SUCCEEDED otherwise behavior will return FAILED.
-  virtual Status onRun(const std::shared_ptr<const typename ActionT::Goal> command) = 0;
+  virtual ResultStatus onRun(const std::shared_ptr<const typename ActionT::Goal> command) = 0;
 
 
   // This is the method derived classes should mainly implement
   // and will be called cyclically while it returns RUNNING.
   // Implement the behavior such that it runs some unit of work on each call
   // and provides a status. The Behavior will finish once SUCCEEDED is returned
   // It's up to the derived class to define the final commanded velocity.
-  virtual Status onCycleUpdate() = 0;
+  virtual ResultStatus onCycleUpdate() = 0;
 
   // an opportunity for derived classes to do something on configuration
   // if they chose
@@ -199,19 +205,20 @@ class TimedBehavior : public nav2_core::Behavior
       return;
     }
 
-    if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) {
+    // Initialize the ActionT result
+    auto result = std::make_shared<typename ActionT::Result>();
+
+    ResultStatus on_run_result = onRun(action_server_->get_current_goal());
+    if (on_run_result.status != Status::SUCCEEDED) {
       RCLCPP_INFO(
         logger_,
         "Initial checks failed for %s", behavior_name_.c_str());
-      action_server_->terminate_current();
+      result->error_code = on_run_result.error_code;
+      action_server_->terminate_current(result);
       return;
     }
 
     auto start_time = steady_clock_.now();
-
-    // Initialize the ActionT result
-    auto result = std::make_shared<typename ActionT::Result>();
-
     rclcpp::WallRate loop_rate(cycle_frequency_);
 
     while (rclcpp::ok()) {
@@ -238,7 +245,8 @@ class TimedBehavior : public nav2_core::Behavior
         return;
       }
 
-      switch (onCycleUpdate()) {
+      ResultStatus on_cycle_update_result = onCycleUpdate();
+      switch (on_cycle_update_result.status) {
         case Status::SUCCEEDED:
           RCLCPP_INFO(
             logger_,
@@ -251,6 +259,7 @@ class TimedBehavior : public nav2_core::Behavior
         case Status::FAILED:
           RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str());
           result->total_elapsed_time = steady_clock_.now() - start_time;
+          result->error_code = on_cycle_update_result.error_code;
           action_server_->terminate_current(result);
           onActionCompletion();
           return;