Skip to content

Commit a42dc34

Browse files
tonynajjarmasf7g
tonynajjar
authored and
masf7g
committed
Revert "Make RecoveryNode return Running (ros-navigation#4777)" (ros-navigation#4790)
This reverts commit 9284c8a.
1 parent 29c3a90 commit a42dc34

File tree

2 files changed

+71
-76
lines changed

2 files changed

+71
-76
lines changed

‎nav2_behavior_tree/plugins/control/recovery_node.cpp‎

Lines changed: 69 additions & 69 deletions
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‎

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,6 @@ TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child)
104104
first_child_->changeStatus(BT::NodeStatus::IDLE);
105105
EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
106106
first_child_->changeStatus(BT::NodeStatus::FAILURE);
107-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
108107
second_child_->changeStatus(BT::NodeStatus::IDLE);
109108
EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
110109
}
@@ -119,9 +118,8 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry)
119118
first_child_->returnSuccessOn(1);
120119
first_child_->changeStatus(BT::NodeStatus::FAILURE);
121120
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
122-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
123-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
124121
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
122+
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS);
125123
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
126124
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
127125
}
@@ -131,17 +129,15 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry)
131129
// first child fails, second child fails
132130
first_child_->changeStatus(BT::NodeStatus::FAILURE);
133131
second_child_->changeStatus(BT::NodeStatus::FAILURE);
134-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
135132
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
133+
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
136134
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
137135
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
138136

139137
// first child fails, second child succeeds, then first child fails (one retry)
140138
first_child_->returnFailureOn(1);
141139
first_child_->changeStatus(BT::NodeStatus::FAILURE);
142140
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
143-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
144-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
145141
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
146142
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
147143
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
@@ -161,7 +157,6 @@ TEST_F(RecoveryNodeTestFixture, test_skipping)
161157
// first child fails, second child skipped
162158
first_child_->changeStatus(BT::NodeStatus::FAILURE);
163159
second_child_->changeStatus(BT::NodeStatus::SKIPPED);
164-
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
165160
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
166161
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
167162
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);

0 commit comments

Comments
 (0)