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.

Sign up for GitHub

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

Revert "Make RecoveryNode return Running" #4790

Merged
merged 1 commit into from
Dec 10, 2024
Merged

Revert "Make RecoveryNode return Running" #4790

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
View file
Open in desktop
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;
}
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;

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
9 changes: 2 additions & 7 deletions nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,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 @@ -120,9 +119,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 @@ -132,17 +130,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 @@ -162,7 +158,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
Loading