Skip to content

Commit 4074d04

Browse files
tonynajjartonynajjar
authored
Revert "Make RecoveryNode return Running (#4777)" (#4790)
This reverts commit 9284c8a.
1 parent d6b02ac commit 4074d04

File tree

2 files changed

+71
-76
lines changed

2 files changed

+71
-76
lines changed

‎nav2_behavior_tree/plugins/control/recovery_node.cpp

+69-69
Original file line numberDiff line numberDiff line change
@@ -37,83 +37,83 @@ BT::NodeStatus RecoveryNode::tick()
3737
throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
3838
}
3939

40-
if (retry_count_ > number_of_retries_) {
41-
halt();
42-
return BT::NodeStatus::FAILURE;
43-
}
4440
setStatus(BT::NodeStatus::RUNNING);
4541

46-
TreeNode * child_node = children_nodes_[current_child_idx_];
47-
const BT::NodeStatus child_status = child_node->executeTick();
48-
49-
if (current_child_idx_ == 0) {
50-
switch (child_status) {
51-
case BT::NodeStatus::SKIPPED:
52-
// If first child is skipped, the entire branch is considered skipped
53-
halt();
54-
return BT::NodeStatus::SKIPPED;
55-
56-
case BT::NodeStatus::SUCCESS:
57-
// reset node and return success when first child returns success
58-
halt();
59-
return BT::NodeStatus::SUCCESS;
60-
61-
case BT::NodeStatus::RUNNING:
62-
return BT::NodeStatus::RUNNING;
63-
64-
case BT::NodeStatus::FAILURE:
65-
{
66-
if (retry_count_ < number_of_retries_) {
67-
// halt first child and tick second child in next iteration
68-
ControlNode::haltChild(0);
69-
current_child_idx_ = 1;
70-
return BT::NodeStatus::RUNNING;
71-
} else {
72-
// reset node and return failure when max retries has been exceeded
73-
halt();
42+
while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) {
43+
TreeNode * child_node = children_nodes_[current_child_idx_];
44+
const BT::NodeStatus child_status = child_node->executeTick();
45+
46+
if (current_child_idx_ == 0) {
47+
switch (child_status) {
48+
case BT::NodeStatus::SKIPPED:
49+
// If first child is skipped, the entire branch is considered skipped
50+
halt();
51+
return BT::NodeStatus::SKIPPED;
52+
53+
case BT::NodeStatus::SUCCESS:
54+
// reset node and return success when first child returns success
55+
halt();
56+
return BT::NodeStatus::SUCCESS;
57+
58+
case BT::NodeStatus::RUNNING:
59+
return BT::NodeStatus::RUNNING;
60+
61+
case BT::NodeStatus::FAILURE:
62+
{
63+
if (retry_count_ < number_of_retries_) {
64+
// halt first child and tick second child in next iteration
65+
ControlNode::haltChild(0);
66+
current_child_idx_++;
67+
break;
68+
} else {
69+
// reset node and return failure when max retries has been exceeded
70+
halt();
71+
return BT::NodeStatus::FAILURE;
72+
}
73+
}
74+
75+
default:
76+
throw BT::LogicError("A child node must never return IDLE");
77+
} // end switch
78+
79+
} else if (current_child_idx_ == 1) {
80+
switch (child_status) {
81+
case BT::NodeStatus::SKIPPED:
82+
{
83+
// if we skip the recovery (maybe a precondition fails), then we
84+
// should assume that no recovery is possible. For this reason,
85+
// we should return FAILURE and reset the index.
86+
// This does not count as a retry.
87+
current_child_idx_ = 0;
88+
ControlNode::haltChild(1);
7489
return BT::NodeStatus::FAILURE;
7590
}
76-
}
77-
78-
default:
79-
throw BT::LogicError("A child node must never return IDLE");
80-
} // end switch
81-
82-
} else if (current_child_idx_ == 1) {
83-
switch (child_status) {
84-
case BT::NodeStatus::SKIPPED:
85-
{
86-
// if we skip the recovery (maybe a precondition fails), then we
87-
// should assume that no recovery is possible. For this reason,
88-
// we should return FAILURE and reset the index.
89-
// This does not count as a retry.
90-
current_child_idx_ = 0;
91-
ControlNode::haltChild(1);
91+
case BT::NodeStatus::RUNNING:
92+
return child_status;
93+
94+
case BT::NodeStatus::SUCCESS:
95+
{
96+
// halt second child, increment recovery count, and tick first child in next iteration
97+
ControlNode::haltChild(1);
98+
retry_count_++;
99+
current_child_idx_ = 0;
100+
}
101+
break;
102+
103+
case BT::NodeStatus::FAILURE:
104+
// reset node and return failure if second child fails
105+
halt();
92106
return BT::NodeStatus::FAILURE;
93-
}
94-
case BT::NodeStatus::RUNNING:
95-
return child_status;
96-
97-
case BT::NodeStatus::SUCCESS:
98-
{
99-
// halt second child, increment recovery count, and tick first child in next iteration
100-
ControlNode::haltChild(1);
101-
retry_count_++;
102-
current_child_idx_ = 0;
103-
return BT::NodeStatus::RUNNING;
104-
}
105107

106-
case BT::NodeStatus::FAILURE:
107-
// reset node and return failure if second child fails
108-
halt();
109-
return BT::NodeStatus::FAILURE;
108+
default:
109+
throw BT::LogicError("A child node must never return IDLE");
110+
} // end switch
111+
}
112+
} // end while loop
110113

111-
default:
112-
throw BT::LogicError("A child node must never return IDLE");
113-
} // end switch
114-
}
114+
// reset node and return failure
115115
halt();
116-
throw BT::LogicError("A recovery node has exactly 2 children and should never reach here");
116+
return BT::NodeStatus::FAILURE;
117117
}
118118

119119
void RecoveryNode::halt()

‎nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp

+2-7
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,6 @@ TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child)
105105
first_child_->changeStatus(BT::NodeStatus::IDLE);
106106
EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
107107
first_child_->changeStatus(BT::NodeStatus::FAILURE);
108-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
109108
second_child_->changeStatus(BT::NodeStatus::IDLE);
110109
EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
111110
}
@@ -120,9 +119,8 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry)
120119
first_child_->returnSuccessOn(1);
121120
first_child_->changeStatus(BT::NodeStatus::FAILURE);
122121
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
123-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
124-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
125122
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
123+
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS);
126124
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
127125
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
128126
}
@@ -132,17 +130,15 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry)
132130
// first child fails, second child fails
133131
first_child_->changeStatus(BT::NodeStatus::FAILURE);
134132
second_child_->changeStatus(BT::NodeStatus::FAILURE);
135-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
136133
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
134+
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
137135
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
138136
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
139137

140138
// first child fails, second child succeeds, then first child fails (one retry)
141139
first_child_->returnFailureOn(1);
142140
first_child_->changeStatus(BT::NodeStatus::FAILURE);
143141
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
144-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
145-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
146142
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
147143
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
148144
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
@@ -162,7 +158,6 @@ TEST_F(RecoveryNodeTestFixture, test_skipping)
162158
// first child fails, second child skipped
163159
first_child_->changeStatus(BT::NodeStatus::FAILURE);
164160
second_child_->changeStatus(BT::NodeStatus::SKIPPED);
165-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
166161
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
167162
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
168163
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);

0 commit comments

Comments
 (0)