From 9baf4675dff475d453f684e0c0cae2da02893b43 Mon Sep 17 00:00:00 2001
From: Sarthak Mittal <sarthakmittal2608@gmail.com>
Date: Fri, 2 Oct 2020 06:46:40 +0530
Subject: [PATCH] Fix reset logic in nav2_behavior_tree control nodes (#2004)
* Fix RecoveryNode logic
Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
* Fix reset logic in all control nodes
Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
* Revert RoundRobin changes
Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
* Revert RoundRobin changes
Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
* Fix RecoveryNode halt
Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
---
.../plugins/control/pipeline_sequence.cpp | 1 +
.../plugins/control/recovery_node.cpp | 25 ++++++++++---------
.../plugins/control/test_recovery_node.cpp | 25 +++++++++++++------
3 files changed, 32 insertions(+), 19 deletions(-)
diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp
index 9b506c68..640c3ed7 100644
--- a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp
+++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp
@@ -69,6 +69,7 @@ BT::NodeStatus PipelineSequence::tick()
void PipelineSequence::halt()
{
BT::ControlNode::halt();
+ last_child_ticked_ = 0;
}
} // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp
index bfe6ac6a..42fc8076 100644
--- a/nav2_behavior_tree/plugins/control/recovery_node.cpp
+++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp
@@ -47,19 +47,21 @@ BT::NodeStatus RecoveryNode::tick()
switch (child_status) {
case BT::NodeStatus::SUCCESS:
{
- retry_count_ = 0;
+ // reset node and return success when first child returns success
halt();
return BT::NodeStatus::SUCCESS;
}
case BT::NodeStatus::FAILURE:
{
- // tick second child
if (retry_count_ < number_of_retries_) {
+ // halt first child and tick second child in next iteration
+ ControlNode::haltChild(0);
current_child_idx_++;
break;
} else {
- ControlNode::haltChildren();
+ // reset node and return failure when max retries has been exceeded
+ halt();
return BT::NodeStatus::FAILURE;
}
}
@@ -71,8 +73,7 @@ BT::NodeStatus RecoveryNode::tick()
default:
{
- halt();
- return BT::NodeStatus::FAILURE;
+ throw BT::LogicError("A child node must never return IDLE");
}
} // end switch
@@ -80,16 +81,16 @@ BT::NodeStatus RecoveryNode::tick()
switch (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_--;
- ControlNode::haltChildren();
}
break;
case BT::NodeStatus::FAILURE:
{
- current_child_idx_--;
- retry_count_ = 0;
+ // reset node and return failure if second child fails
halt();
return BT::NodeStatus::FAILURE;
}
@@ -101,13 +102,13 @@ BT::NodeStatus RecoveryNode::tick()
default:
{
- halt();
- return BT::NodeStatus::FAILURE;
+ throw BT::LogicError("A child node must never return IDLE");
}
} // end switch
}
} // end while loop
- retry_count_ = 0;
+
+ // reset node and return failure
halt();
return BT::NodeStatus::FAILURE;
}
@@ -115,8 +116,8 @@ BT::NodeStatus RecoveryNode::tick()
void RecoveryNode::halt()
{
ControlNode::halt();
- current_child_idx_ = 0;
retry_count_ = 0;
+ current_child_idx_ = 0;
}
} // namespace nav2_behavior_tree
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 8356a006..608d8f58 100644
--- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp
+++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp
@@ -26,21 +26,31 @@ class RecoveryDummy : public nav2_behavior_tree::DummyNode
public:
BT::NodeStatus tick() override
{
- if (++ticks_ > num_failures_ && num_failures_ != -1) {
+ if (ticks_ == num_success_) {
setStatus(BT::NodeStatus::SUCCESS);
+ } else if (ticks_ == num_failure_) {
+ setStatus(BT::NodeStatus::FAILURE);
}
+ ticks_++;
return status();
}
- void setMaxFailures(int max_failures)
+ void returnSuccessOn(int tick)
{
- num_failures_ = max_failures;
+ num_success_ = tick;
+ ticks_ = 0;
+ }
+
+ void returnFailureOn(int tick)
+ {
+ num_failure_ = tick;
ticks_ = 0;
}
private:
int ticks_{0};
- int num_failures_{-1};
+ int num_success_{-1};
+ int num_failure_{-1};
};
class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
@@ -92,10 +102,10 @@ TEST_F(RecoveryNodeTestFixture, test_running)
TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child)
{
first_child_->changeStatus(BT::NodeStatus::IDLE);
- EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+ EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
first_child_->changeStatus(BT::NodeStatus::FAILURE);
second_child_->changeStatus(BT::NodeStatus::IDLE);
- EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+ EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
}
TEST_F(RecoveryNodeTestFixture, test_success_one_retry)
@@ -105,7 +115,7 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry)
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
// first child fails, second child succeeds, then first child succeeds (one retry)
- first_child_->setMaxFailures(1);
+ first_child_->returnSuccessOn(1);
first_child_->changeStatus(BT::NodeStatus::FAILURE);
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
@@ -125,6 +135,7 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry)
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::FAILURE);
--
GitLab