Add Recovery timeout (#25)

* Add recovery node timeout * Fix is_timeout_running_ * Fix timeout default value * Fix comma * Fix typo * Use shared node timer and fix timeout * Fix typo * Fix last_recovery_time_ setting and clock type mismatch --------- Co-authored-by: emilnovak <[email protected]>
EnjoyRobotics · Oct 8, 2024 · edaf470 · edaf470
1 parent 0254af6
commit edaf470
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 2 deletions.
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp
@@ -16,6 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
 
 #include <string>
+#include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp_v3/control_node.h"
 
 namespace nav2_behavior_tree
@@ -56,14 +57,20 @@ class RecoveryNode : public BT::ControlNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<int>("number_of_retries", 1, "Number of retries")
+      BT::InputPort<int>("number_of_retries", 1, "Number of retries"),
+      BT::InputPort<int>(
+        "timeout", 0,
+        "Timeout in (uint) seconds for resetting retry count (0 - disbale)")
     };
   }
 
 private:
+  rclcpp::Node::SharedPtr node_;
   unsigned int current_child_idx_;
   unsigned int number_of_retries_;
+  unsigned int timeout_;
   unsigned int retry_count_;
+  rclcpp::Time last_recovery_time_;
 
   /**
    * @brief The main override required by a BT action

diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -266,6 +266,7 @@
 
     <Control ID="RecoveryNode">
       <input_port name="number_of_retries">Number of retries</input_port>
+      <input_port name="timeout">Timeout in (uint) seconds for resetting retry count (0 - disable)</input_port>
     </Control>
 
     <Control ID="RoundRobin"/>

diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp
@@ -24,9 +24,12 @@ RecoveryNode::RecoveryNode(
 : BT::ControlNode::ControlNode(name, conf),
   current_child_idx_(0),
   number_of_retries_(1),
-  retry_count_(0)
+  retry_count_(0),
+  last_recovery_time_(rclcpp::Time(0, 0, RCL_ROS_TIME))
 {
   getInput("number_of_retries", number_of_retries_);
+  getInput("timeout", timeout_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 }
 
 BT::NodeStatus RecoveryNode::tick()
@@ -39,6 +42,13 @@ BT::NodeStatus RecoveryNode::tick()
 
   setStatus(BT::NodeStatus::RUNNING);
 
+  // if reached timeout reset retry_count_
+  if (timeout_ > 0 &&
+    node_->now() > last_recovery_time_ + std::chrono::seconds(timeout_))
+  {
+    retry_count_ = 0;
+  }
+
   while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) {
     TreeNode * child_node = children_nodes_[current_child_idx_];
     const BT::NodeStatus child_status = child_node->executeTick();
@@ -83,6 +93,7 @@ BT::NodeStatus RecoveryNode::tick()
           {
             // halt second child, increment recovery count, and tick first child in next iteration
             ControlNode::haltChild(1);
+            last_recovery_time_ = node_->now();
             retry_count_++;
             current_child_idx_--;
           }
@@ -118,6 +129,7 @@ void RecoveryNode::halt()
   ControlNode::halt();
   retry_count_ = 0;
   current_child_idx_ = 0;
+  last_recovery_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
 }
 
 }  // namespace nav2_behavior_tree