Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make RecoveryNode return Running #4777

Merged
merged 24 commits into from
Dec 4, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
138 changes: 69 additions & 69 deletions nav2_behavior_tree/plugins/control/recovery_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,83 +37,83 @@
throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
}

if (retry_count_ > number_of_retries_) {
halt();
return BT::NodeStatus::FAILURE;

Check warning on line 42 in nav2_behavior_tree/plugins/control/recovery_node.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_behavior_tree/plugins/control/recovery_node.cpp#L41-L42

Added lines #L41 - L42 were not covered by tests
}
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");

Check warning on line 116 in nav2_behavior_tree/plugins/control/recovery_node.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_behavior_tree/plugins/control/recovery_node.cpp#L116

Added line #L116 was not covered by tests
}

void RecoveryNode::halt()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand All @@ -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);
}
Expand All @@ -130,15 +132,17 @@ 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 @@ -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);
Expand Down
Loading