Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Commit d17a078

Browse files
renan028renan028
authored
add failure threshold to parallel node with tests (BehaviorTree#216)
1 parent 73c1574 commit d17a078

File tree

3 files changed

+98
-18
lines changed

3 files changed

+98
-18
lines changed

‎include/behaviortree_cpp_v3/controls/parallel_node.h

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -24,29 +24,35 @@ class ParallelNode : public ControlNode
2424
{
2525
public:
2626

27-
ParallelNode(const std::string& name, unsigned threshold);
27+
ParallelNode(const std::string& name, unsigned success_threshold,
28+
unsigned failure_threshold = 1);
2829

2930
ParallelNode(const std::string& name, const NodeConfiguration& config);
3031

3132
static PortsList providedPorts()
3233
{
33-
return { InputPort<unsigned>(THRESHOLD_KEY) };
34+
return { InputPort<unsigned>(THRESHOLD_SUCCESS),
35+
InputPort<unsigned>(THRESHOLD_FAILURE) };
3436
}
3537

3638
~ParallelNode() = default;
3739

3840
virtual void halt() override;
3941

4042
unsigned int thresholdM();
43+
unsigned int thresholdFM();
4144
void setThresholdM(unsigned int threshold_M);
45+
void setThresholdFM(unsigned int threshold_M);
4246

4347
private:
44-
unsigned int threshold_;
48+
unsigned int success_threshold_;
49+
unsigned int failure_threshold_;
4550

4651
std::set<int> skip_list_;
4752

4853
bool read_parameter_from_ports_;
49-
static constexpr const char* THRESHOLD_KEY = "threshold";
54+
static constexpr const char* THRESHOLD_SUCCESS = "success_threshold";
55+
static constexpr const char* THRESHOLD_FAILURE = "failure_threshold";
5056

5157
virtual BT::NodeStatus tick() override;
5258
};

‎src/controls/parallel_node.cpp

Lines changed: 40 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,14 @@
1616
namespace BT
1717
{
1818

19-
constexpr const char* ParallelNode::THRESHOLD_KEY;
19+
constexpr const char* ParallelNode::THRESHOLD_FAILURE;
20+
constexpr const char* ParallelNode::THRESHOLD_SUCCESS;
2021

21-
ParallelNode::ParallelNode(const std::string& name, unsigned threshold)
22+
ParallelNode::ParallelNode(const std::string& name, unsigned success_threshold,
23+
unsigned failure_threshold)
2224
: ControlNode::ControlNode(name, {} ),
23-
threshold_(threshold),
25+
success_threshold_(success_threshold),
26+
failure_threshold_(failure_threshold),
2427
read_parameter_from_ports_(false)
2528
{
2629
setRegistrationID("Parallel");
@@ -29,7 +32,8 @@ ParallelNode::ParallelNode(const std::string& name, unsigned threshold)
2932
ParallelNode::ParallelNode(const std::string &name,
3033
const NodeConfiguration& config)
3134
: ControlNode::ControlNode(name, config),
32-
threshold_(0),
35+
success_threshold_(1),
36+
failure_threshold_(1),
3337
read_parameter_from_ports_(true)
3438
{
3539
}
@@ -38,9 +42,14 @@ NodeStatus ParallelNode::tick()
3842
{
3943
if(read_parameter_from_ports_)
4044
{
41-
if( !getInput(THRESHOLD_KEY, threshold_) )
45+
if( !getInput(THRESHOLD_SUCCESS, success_threshold_) )
4246
{
43-
throw RuntimeError("Missing parameter [", THRESHOLD_KEY, "] in ParallelNode");
47+
throw RuntimeError("Missing parameter [", THRESHOLD_SUCCESS, "] in ParallelNode");
48+
}
49+
50+
if( !getInput(THRESHOLD_FAILURE, failure_threshold_) )
51+
{
52+
throw RuntimeError("Missing parameter [", THRESHOLD_FAILURE, "] in ParallelNode");
4453
}
4554
}
4655

@@ -49,9 +58,14 @@ NodeStatus ParallelNode::tick()
4958

5059
const size_t children_count = children_nodes_.size();
5160

52-
if( children_count < threshold_)
61+
if( children_count < success_threshold_)
5362
{
54-
throw LogicError("Number of children is less than threshold. Can never suceed.");
63+
throw LogicError("Number of children is less than threshold. Can never succeed.");
64+
}
65+
66+
if( children_count < failure_threshold_)
67+
{
68+
throw LogicError("Number of children is less than threshold. Can never fail.");
5569
}
5670

5771
// Routing the tree according to the sequence node's logic:
@@ -80,7 +94,7 @@ NodeStatus ParallelNode::tick()
8094
}
8195
success_childred_num++;
8296

83-
if (success_childred_num == threshold_)
97+
if (success_childred_num == success_threshold_)
8498
{
8599
skip_list_.clear();
86100
haltChildren();
@@ -95,8 +109,11 @@ NodeStatus ParallelNode::tick()
95109
skip_list_.insert(i);
96110
}
97111
failure_childred_num++;
98-
99-
if (failure_childred_num > children_count - threshold_)
112+
113+
// It fails if it is not possible to succeed anymore or if
114+
// number of failures are equal to failure_threshold_
115+
if ((failure_childred_num > children_count - success_threshold_)
116+
|| (failure_childred_num == failure_threshold_))
100117
{
101118
skip_list_.clear();
102119
haltChildren();
@@ -127,12 +144,22 @@ void ParallelNode::halt()
127144

128145
unsigned int ParallelNode::thresholdM()
129146
{
130-
return threshold_;
147+
return success_threshold_;
148+
}
149+
150+
unsigned int ParallelNode::thresholdFM()
151+
{
152+
return failure_threshold_;
131153
}
132154

133155
void ParallelNode::setThresholdM(unsigned int threshold_M)
134156
{
135-
threshold_ = threshold_M;
157+
success_threshold_ = threshold_M;
158+
}
159+
160+
void ParallelNode::setThresholdFM(unsigned int threshold_M)
161+
{
162+
failure_threshold_ = threshold_M;
136163
}
137164

138165
}

‎tests/gtest_parallel.cpp

Lines changed: 48 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -145,7 +145,7 @@ TEST_F(SimpleParallelTest, Threshold_3)
145145
ASSERT_EQ(NodeStatus::SUCCESS, state);
146146
}
147147

148-
TEST_F(SimpleParallelTest, Threshold_1)
148+
TEST_F(SimpleParallelTest, Threshold_2)
149149
{
150150
root.setThresholdM(2);
151151
BT::NodeStatus state = root.executeTick();
@@ -189,11 +189,57 @@ TEST_F(ComplexParallelTest, ConditionsTrue)
189189
ASSERT_EQ(NodeStatus::SUCCESS, state);
190190
}
191191

192+
TEST_F(ComplexParallelTest, ConditionsLeftFalse)
193+
{
194+
parallel_left.setThresholdFM(3);
195+
parallel_left.setThresholdM(3);
196+
condition_L1.setExpectedResult(NodeStatus::FAILURE);
197+
condition_L2.setExpectedResult(NodeStatus::FAILURE);
198+
BT::NodeStatus state = parallel_root.executeTick();
199+
200+
// It fails because Parallel Left it will never succeed (two already fail)
201+
// even though threshold_failure == 3
202+
203+
ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
204+
ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
205+
ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
206+
ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
207+
ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
208+
209+
ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
210+
ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
211+
ASSERT_EQ(NodeStatus::IDLE, action_R.status());
212+
213+
ASSERT_EQ(NodeStatus::FAILURE, state);
214+
}
215+
192216
TEST_F(ComplexParallelTest, ConditionRightFalse)
193217
{
194218
condition_R.setExpectedResult(NodeStatus::FAILURE);
195219
BT::NodeStatus state = parallel_root.executeTick();
196220

221+
// It fails because threshold_failure is 1 for parallel right and
222+
// condition_R fails
223+
224+
ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
225+
ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
226+
ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
227+
ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
228+
ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
229+
230+
ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
231+
ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
232+
ASSERT_EQ(NodeStatus::IDLE, action_R.status());
233+
234+
ASSERT_EQ(NodeStatus::FAILURE, state);
235+
}
236+
237+
TEST_F(ComplexParallelTest, ConditionRightFalse_thresholdF_2)
238+
{
239+
parallel_right.setThresholdFM(2);
240+
condition_R.setExpectedResult(NodeStatus::FAILURE);
241+
BT::NodeStatus state = parallel_root.executeTick();
242+
197243
// All the actions are running
198244

199245
ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
@@ -229,6 +275,7 @@ TEST_F(ComplexParallelTest, ConditionRightFalseAction1Done)
229275
{
230276
condition_R.setExpectedResult(NodeStatus::FAILURE);
231277

278+
parallel_right.setThresholdFM(2);
232279
parallel_left.setThresholdM(4);
233280

234281
BT::NodeStatus state = parallel_root.executeTick();

0 commit comments

Comments
 (0)