Skip to content

Commit

Permalink
Revert "Make RecoveryNode return Running (ros-navigation#4777)" (ros-…
Browse files Browse the repository at this point in the history 
…navigation#4790)

This reverts commit 9284c8a.
  • Loading branch information
@tonynajjar @masf7g
tonynajjar authored and masf7g committed Dec 19, 2024
1 parent 29c3a90 commit a42dc34
Show file tree
Hide file tree
Showing 2 changed files with 71 additions and 76 deletions.
138 changes: 69 additions & 69 deletions nav2_behavior_tree/plugins/control/recovery_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

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();
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);
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);
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();
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;
}

case BT::NodeStatus::FAILURE:
// reset node and return failure if second child fails
halt();
return BT::NodeStatus::FAILURE;
default:
throw BT::LogicError("A child node must never return IDLE");
} // end switch
}
} // end while loop

default:
throw BT::LogicError("A child node must never return IDLE");
} // end switch
}
// reset node and return failure
halt();
throw BT::LogicError("A recovery node has exactly 2 children and should never reach here");
return BT::NodeStatus::FAILURE;
}

void RecoveryNode::halt()
Expand Down
9 changes: 2 additions & 7 deletions nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,6 @@ 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);
}
Expand All @@ -119,9 +118,8 @@ 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);
}
Expand All @@ -131,17 +129,15 @@ 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);

// first child fails, second child succeeds, then first child fails (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);
Expand All @@ -161,7 +157,6 @@ 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);
Expand Down

0 comments on commit a42dc34

Please sign in to comment.