diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 6eb3c6e99e5..1ab9d2ff34c 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -37,83 +37,83 @@ BT::NodeStatus RecoveryNode::tick() throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children."); } + if (retry_count_ > number_of_retries_) { + halt(); + return BT::NodeStatus::FAILURE; + } setStatus(BT::NodeStatus::RUNNING); - 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(); - - if (current_child_idx_ == 0) { - switch (child_status) { - case BT::NodeStatus::SKIPPED: - // If first child is skipped, the entire branch is considered skipped - halt(); - return BT::NodeStatus::SKIPPED; - - case BT::NodeStatus::SUCCESS: - // reset node and return success when first child returns success - halt(); - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::FAILURE: - { - if (retry_count_ < number_of_retries_) { - // halt first child and tick second child in next iteration - ControlNode::haltChild(0); - current_child_idx_++; - break; - } else { - // reset node and return failure when max retries has been exceeded - halt(); - return BT::NodeStatus::FAILURE; - } - } - - default: - throw BT::LogicError("A child node must never return IDLE"); - } // end switch - - } else if (current_child_idx_ == 1) { - switch (child_status) { - case BT::NodeStatus::SKIPPED: - { - // if we skip the recovery (maybe a precondition fails), then we - // should assume that no recovery is possible. For this reason, - // we should return FAILURE and reset the index. - // This does not count as a retry. - current_child_idx_ = 0; - ControlNode::haltChild(1); + TreeNode * child_node = children_nodes_[current_child_idx_]; + const BT::NodeStatus child_status = child_node->executeTick(); + + if (current_child_idx_ == 0) { + switch (child_status) { + case BT::NodeStatus::SKIPPED: + // If first child is skipped, the entire branch is considered skipped + halt(); + return BT::NodeStatus::SKIPPED; + + case BT::NodeStatus::SUCCESS: + // reset node and return success when first child returns success + halt(); + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::FAILURE: + { + if (retry_count_ < number_of_retries_) { + // halt first child and tick second child in next iteration + ControlNode::haltChild(0); + current_child_idx_ = 1; + return BT::NodeStatus::RUNNING; + } else { + // reset node and return failure when max retries has been exceeded + halt(); return BT::NodeStatus::FAILURE; } - case BT::NodeStatus::RUNNING: - return child_status; - - case BT::NodeStatus::SUCCESS: - { - // halt second child, increment recovery count, and tick first child in next iteration - ControlNode::haltChild(1); - retry_count_++; - current_child_idx_ = 0; - } - break; - - case BT::NodeStatus::FAILURE: - // reset node and return failure if second child fails - halt(); + } + + default: + throw BT::LogicError("A child node must never return IDLE"); + } // end switch + + } else if (current_child_idx_ == 1) { + switch (child_status) { + case BT::NodeStatus::SKIPPED: + { + // if we skip the recovery (maybe a precondition fails), then we + // should assume that no recovery is possible. For this reason, + // we should return FAILURE and reset the index. + // This does not count as a retry. + current_child_idx_ = 0; + ControlNode::haltChild(1); return BT::NodeStatus::FAILURE; + } + case BT::NodeStatus::RUNNING: + return child_status; + + case BT::NodeStatus::SUCCESS: + { + // halt second child, increment recovery count, and tick first child in next iteration + ControlNode::haltChild(1); + retry_count_++; + current_child_idx_ = 0; + return BT::NodeStatus::RUNNING; + } - default: - throw BT::LogicError("A child node must never return IDLE"); - } // end switch - } - } // end while loop + case BT::NodeStatus::FAILURE: + // reset node and return failure if second child fails + halt(); + return BT::NodeStatus::FAILURE; - // reset node and return failure + default: + throw BT::LogicError("A child node must never return IDLE"); + } // end switch + } halt(); - return BT::NodeStatus::FAILURE; + throw BT::LogicError("A recovery node has exactly 2 children and should never reach here"); } void RecoveryNode::halt() diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index 82927031429..532fcad45b2 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -105,6 +105,7 @@ TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child) first_child_->changeStatus(BT::NodeStatus::IDLE); EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); first_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); second_child_->changeStatus(BT::NodeStatus::IDLE); EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); } @@ -119,8 +120,9 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry) first_child_->returnSuccessOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); } @@ -130,8 +132,8 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) // first child fails, second child fails first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); - EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); @@ -139,6 +141,8 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) first_child_->returnFailureOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); @@ -158,6 +162,7 @@ TEST_F(RecoveryNodeTestFixture, test_skipping) // first child fails, second child skipped first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);