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 +#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("number_of_retries", 1, "Number of retries") + BT::InputPort("number_of_retries", 1, "Number of retries"), + BT::InputPort( + "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 @@ Number of retries + Timeout in (uint) seconds for resetting retry count (0 - disable) 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("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