Skip to content

Commit 9284c8a

Browse files
tonynajjartonynajjar
authored
Make RecoveryNode return Running (ros-navigation#4777)
* 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>
1 parent 3ee384d commit 9284c8a

File tree

2 files changed

+76
-71
lines changed

2 files changed

+76
-71
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+
}
4044
setStatus(BT::NodeStatus::RUNNING);
4145

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);
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();
8974
return BT::NodeStatus::FAILURE;
9075
}
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();
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);
10692
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+
}
107105

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

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

119119
void RecoveryNode::halt()

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

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -105,6 +105,7 @@ 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);
108109
second_child_->changeStatus(BT::NodeStatus::IDLE);
109110
EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
110111
}
@@ -119,8 +120,9 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry)
119120
first_child_->returnSuccessOn(1);
120121
first_child_->changeStatus(BT::NodeStatus::FAILURE);
121122
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
123+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
124+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
122125
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
123-
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS);
124126
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
125127
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
126128
}
@@ -130,15 +132,17 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry)
130132
// first child fails, second child fails
131133
first_child_->changeStatus(BT::NodeStatus::FAILURE);
132134
second_child_->changeStatus(BT::NodeStatus::FAILURE);
135+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
133136
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
134-
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
135137
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
136138
EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
137139

138140
// first child fails, second child succeeds, then first child fails (one retry)
139141
first_child_->returnFailureOn(1);
140142
first_child_->changeStatus(BT::NodeStatus::FAILURE);
141143
second_child_->changeStatus(BT::NodeStatus::SUCCESS);
144+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
145+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
142146
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
143147
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
144148
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
@@ -158,6 +162,7 @@ TEST_F(RecoveryNodeTestFixture, test_skipping)
158162
// first child fails, second child skipped
159163
first_child_->changeStatus(BT::NodeStatus::FAILURE);
160164
second_child_->changeStatus(BT::NodeStatus::SKIPPED);
165+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
161166
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
162167
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
163168
EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);

0 commit comments

Comments
 (0)