Skip to content

Commit 2dc84b1

Browse files
author
Davide Faconti
committed
make loops preemptable
1 parent f590d0f commit 2dc84b1

File tree

5 files changed

+152
-154
lines changed

5 files changed

+152
-154
lines changed

‎src/controls/fallback_node.cpp

Lines changed: 30 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,8 @@ namespace BT
1717
{
1818

1919
FallbackNode::FallbackNode(const std::string& name)
20-
: ControlNode::ControlNode(name, {} )
21-
,current_child_idx_(0)
20+
: ControlNode::ControlNode(name, {} )
21+
,current_child_idx_(0)
2222
{
2323
setRegistrationID("Fallback");
2424
}
@@ -29,44 +29,42 @@ NodeStatus FallbackNode::tick()
2929

3030
setStatus(NodeStatus::RUNNING);
3131

32-
while (current_child_idx_ < children_count)
33-
{
34-
TreeNode* current_child_node = children_nodes_[current_child_idx_];
35-
const NodeStatus child_status = current_child_node->executeTick();
3632

37-
switch (child_status)
33+
TreeNode* current_child_node = children_nodes_[current_child_idx_];
34+
const NodeStatus child_status = current_child_node->executeTick();
35+
36+
switch (child_status)
37+
{
38+
case NodeStatus::RUNNING:
39+
{
40+
return child_status;
41+
}
42+
case NodeStatus::SUCCESS:
3843
{
39-
case NodeStatus::RUNNING:
44+
haltChildren(0);
45+
current_child_idx_ = 0;
46+
return child_status;
47+
}
48+
case NodeStatus::FAILURE:
49+
{
50+
current_child_idx_++;
51+
if( current_child_idx_ < children_count )
4052
{
41-
return child_status;
53+
return NodeStatus::RUNNING;
4254
}
43-
case NodeStatus::SUCCESS:
44-
{
55+
else{
4556
haltChildren(0);
4657
current_child_idx_ = 0;
47-
return child_status;
48-
}
49-
case NodeStatus::FAILURE:
50-
{
51-
current_child_idx_++;
52-
}
53-
break;
54-
55-
case NodeStatus::IDLE:
56-
{
57-
throw LogicError("A child node must never return IDLE");
58+
return NodeStatus::FAILURE;
5859
}
59-
} // end switch
60-
} // end while loop
60+
}
6161

62-
// The entire while loop completed. This means that all the children returned FAILURE.
63-
if (current_child_idx_ == children_count)
64-
{
65-
haltChildren(0);
66-
current_child_idx_ = 0;
67-
}
68-
69-
return NodeStatus::FAILURE;
62+
case NodeStatus::IDLE:
63+
{
64+
throw LogicError("A child node must never return IDLE");
65+
}
66+
} // end switch
67+
// never reached
7068
}
7169

7270
void FallbackNode::halt()

‎src/controls/sequence_node.cpp

Lines changed: 29 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ namespace BT
1919

2020

2121
SequenceNode::SequenceNode(const std::string& name)
22-
: ControlNode::ControlNode(name, {} )
22+
: ControlNode::ControlNode(name, {} )
2323
, current_child_idx_(0)
2424
{
2525
setRegistrationID("Sequence");
@@ -37,44 +37,42 @@ NodeStatus SequenceNode::tick()
3737

3838
setStatus(NodeStatus::RUNNING);
3939

40-
while (current_child_idx_ < children_count)
41-
{
42-
TreeNode* current_child_node = children_nodes_[current_child_idx_];
43-
const NodeStatus child_status = current_child_node->executeTick();
40+
TreeNode* current_child_node = children_nodes_[current_child_idx_];
41+
const NodeStatus child_status = current_child_node->executeTick();
4442

45-
switch (child_status)
43+
switch (child_status)
44+
{
45+
case NodeStatus::RUNNING:
46+
{
47+
return child_status;
48+
}
49+
case NodeStatus::FAILURE:
4650
{
47-
case NodeStatus::RUNNING:
51+
// Reset on failure
52+
haltChildren(0);
53+
current_child_idx_ = 0;
54+
return child_status;
55+
}
56+
case NodeStatus::SUCCESS:
57+
{
58+
current_child_idx_++;
59+
if( current_child_idx_ < children_count )
4860
{
49-
return child_status;
61+
return NodeStatus::RUNNING;
5062
}
51-
case NodeStatus::FAILURE:
52-
{
53-
// Reset on failure
63+
else{
5464
haltChildren(0);
5565
current_child_idx_ = 0;
56-
return child_status;
57-
}
58-
case NodeStatus::SUCCESS:
59-
{
60-
current_child_idx_++;
61-
}
62-
break;
63-
64-
case NodeStatus::IDLE:
65-
{
66-
throw LogicError("A child node must never return IDLE");
66+
return NodeStatus::SUCCESS;
6767
}
68-
} // end switch
69-
} // end while loop
68+
}
7069

71-
// The entire while loop completed. This means that all the children returned SUCCESS.
72-
if (current_child_idx_ == children_count)
73-
{
74-
haltChildren(0);
75-
current_child_idx_ = 0;
76-
}
77-
return NodeStatus::SUCCESS;
70+
case NodeStatus::IDLE:
71+
{
72+
throw LogicError("A child node must never return IDLE");
73+
}
74+
} // end switch
75+
// never reached
7876
}
7977

8078
}

‎src/controls/sequence_star_node.cpp

Lines changed: 30 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ namespace BT
1717
{
1818

1919
SequenceStarNode::SequenceStarNode(const std::string& name)
20-
: ControlNode::ControlNode(name, {} )
20+
: ControlNode::ControlNode(name, {} )
2121
, current_child_idx_(0)
2222
{
2323
setRegistrationID("SequenceStar");
@@ -29,43 +29,41 @@ NodeStatus SequenceStarNode::tick()
2929

3030
setStatus(NodeStatus::RUNNING);
3131

32-
while (current_child_idx_ < children_count)
33-
{
34-
TreeNode* current_child_node = children_nodes_[current_child_idx_];
35-
const NodeStatus child_status = current_child_node->executeTick();
32+
TreeNode* current_child_node = children_nodes_[current_child_idx_];
33+
const NodeStatus child_status = current_child_node->executeTick();
3634

37-
switch (child_status)
35+
switch (child_status)
36+
{
37+
case NodeStatus::RUNNING:
3838
{
39-
case NodeStatus::RUNNING:
40-
{
41-
return child_status;
42-
}
43-
case NodeStatus::FAILURE:
44-
{
45-
// DO NOT reset on failure
46-
haltChildren(current_child_idx_);
47-
return child_status;
48-
}
49-
case NodeStatus::SUCCESS:
39+
return child_status;
40+
}
41+
case NodeStatus::FAILURE:
42+
{
43+
// DO NOT reset on failure
44+
haltChildren(current_child_idx_);
45+
return child_status;
46+
}
47+
case NodeStatus::SUCCESS:
48+
{
49+
current_child_idx_++;
50+
if( current_child_idx_ < children_count )
5051
{
51-
current_child_idx_++;
52+
return NodeStatus::RUNNING;
5253
}
53-
break;
54-
55-
case NodeStatus::IDLE:
56-
{
57-
throw LogicError("A child node must never return IDLE");
54+
else{
55+
haltChildren(0);
56+
current_child_idx_ = 0;
57+
return NodeStatus::SUCCESS;
5858
}
59-
} // end switch
60-
} // end while loop
59+
}
6160

62-
// The entire while loop completed. This means that all the children returned SUCCESS.
63-
if (current_child_idx_ == children_count)
64-
{
65-
haltChildren(0);
66-
current_child_idx_ = 0;
67-
}
68-
return NodeStatus::SUCCESS;
61+
case NodeStatus::IDLE:
62+
{
63+
throw LogicError("A child node must never return IDLE");
64+
}
65+
} // end switch
66+
// never reached
6967
}
7068

7169
void SequenceStarNode::halt()

‎src/decorators/repeat_node.cpp

Lines changed: 32 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -18,19 +18,19 @@ namespace BT
1818
constexpr const char* RepeatNode::NUM_CYCLES;
1919

2020
RepeatNode::RepeatNode(const std::string& name, unsigned int NTries)
21-
: DecoratorNode(name, {} ),
22-
num_cycles_(NTries),
23-
try_index_(0),
24-
read_parameter_from_ports_(false)
21+
: DecoratorNode(name, {} ),
22+
num_cycles_(NTries),
23+
try_index_(0),
24+
read_parameter_from_ports_(false)
2525
{
26-
setRegistrationID("Repeat");
26+
setRegistrationID("Repeat");
2727
}
2828

2929
RepeatNode::RepeatNode(const std::string& name, const NodeConfiguration& config)
3030
: DecoratorNode(name, config),
31-
num_cycles_(0),
32-
try_index_(0),
33-
read_parameter_from_ports_(true)
31+
num_cycles_(0),
32+
try_index_(0),
33+
read_parameter_from_ports_(true)
3434
{
3535

3636
}
@@ -47,38 +47,40 @@ NodeStatus RepeatNode::tick()
4747

4848
setStatus(NodeStatus::RUNNING);
4949

50-
while (try_index_ < num_cycles_)
51-
{
52-
NodeStatus child_state = child_node_->executeTick();
50+
NodeStatus child_state = child_node_->executeTick();
5351

54-
switch (child_state)
52+
switch (child_state)
53+
{
54+
case NodeStatus::SUCCESS:
5555
{
56-
case NodeStatus::SUCCESS:
56+
try_index_++;
57+
if( try_index_ < num_cycles_)
5758
{
58-
try_index_++;
59+
return NodeStatus::RUNNING;
5960
}
60-
break;
61-
62-
case NodeStatus::FAILURE:
63-
{
61+
else{
6462
try_index_ = 0;
65-
return (NodeStatus::FAILURE);
63+
return NodeStatus::SUCCESS;
6664
}
65+
}
6766

68-
case NodeStatus::RUNNING:
69-
{
70-
return NodeStatus::RUNNING;
71-
}
67+
case NodeStatus::FAILURE:
68+
{
69+
try_index_ = 0;
70+
return (NodeStatus::FAILURE);
71+
}
7272

73-
default:
74-
{
75-
throw LogicError("A child node must never return IDLE");
76-
}
73+
case NodeStatus::RUNNING:
74+
{
75+
return NodeStatus::RUNNING;
7776
}
78-
}
7977

80-
try_index_ = 0;
81-
return NodeStatus::SUCCESS;
78+
default:
79+
{
80+
throw LogicError("A child node must never return IDLE");
81+
}
82+
}
83+
// never reached
8284
}
8385

8486
void RepeatNode::halt()

0 commit comments

Comments
 (0)