From edaf470ab4eb4256c7b3ac73914ecde5a9ba575e Mon Sep 17 00:00:00 2001
From: Balint Rozgonyi <43723477+RBT22@users.noreply.github.com>
Date: Tue, 8 Oct 2024 12:23:36 +0200
Subject: [PATCH] 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 <emil.tibor.novak@gmail.com>
---
 .../plugins/control/recovery_node.hpp              |  9 ++++++++-
 nav2_behavior_tree/nav2_tree_nodes.xml             |  1 +
 .../plugins/control/recovery_node.cpp              | 14 +++++++++++++-
 3 files changed, 22 insertions(+), 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
index 89c0cfa300c..8114c7fffc3 100644
--- 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
index 360a3bea608..d21908a126a 100644
--- 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
index 42fc8076be6..2c90e2e4c6d 100644
--- 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