Skip to content

Commit

Permalink
Make RecoveryNode return Running (ros-navigation#4777)
Browse files Browse the repository at this point in the history 
* Add IsStoppedBTNode

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add topic name + reformat

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix comment

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix abs

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove log

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add getter functions for raw twist

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove unused code

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* use odomsmoother

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix formatting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* update groot

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add test

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* reset at success

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* FIX velocity_threshold_

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix stopped Node

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add tests  to odometry_utils

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Make RecoveryNode return RUNNING

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* PR review

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add halt at the end

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
Signed-off-by: Joseph Duchesne <josephgeek@gmail.com>
  • Loading branch information
@tonynajjar @josephduchesne
tonynajjar authored and josephduchesne committed Dec 10, 2024
1 parent 0bef9e8 commit 7f25745
Show file tree
Hide file tree
Showing 2 changed files with 76 additions and 71 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);

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()
Expand Down
9 changes: 7 additions & 2 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 @@ -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

0 comments on commit 7f25745

Please sign in to comment.