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"