|
13 | 13 |
|
14 | 14 | #include "behavior_tree_core/controls/sequence_node.h" |
15 | 15 |
|
16 | | -BT::SequenceNode::SequenceNode(const std::string& name) : ControlNode::ControlNode(name, NodeParameters()) |
| 16 | +namespace BT |
17 | 17 | { |
18 | | -} |
19 | 18 |
|
20 | | -BT::NodeStatus BT::SequenceNode::tick() |
| 19 | +SequenceNode::SequenceNode(const std::string& name) : ControlNode::ControlNode(name, NodeParameters()) |
21 | 20 | { |
22 | | - // gets the number of children. The number could change if, at runtime, one edits the tree. |
23 | | - const unsigned N_of_children = children_nodes_.size(); |
| 21 | +} |
24 | 22 |
|
25 | | - // Routing the ticks according to the sequence node's logic: |
| 23 | +NodeStatus SequenceNode::tick() |
| 24 | +{ |
| 25 | + const unsigned children_count = children_nodes_.size(); |
26 | 26 |
|
27 | 27 | setStatus(NodeStatus::RUNNING); |
28 | 28 |
|
29 | | - for (unsigned int i = 0; i < N_of_children; i++) |
| 29 | + for (unsigned int index = 0; index < children_count; index++) |
30 | 30 | { |
31 | | - TreeNode* child_node = children_nodes_[i]; |
32 | | - |
| 31 | + TreeNode* child_node = children_nodes_[index]; |
33 | 32 | const NodeStatus child_status = child_node->executeTick(); |
34 | 33 |
|
35 | | - // Ponderate on which status to send to the parent |
36 | | - if (child_status != NodeStatus::SUCCESS) |
| 34 | + switch( child_status ) |
37 | 35 | { |
38 | | - // If the child status is not success, halt the next children and return the status to your parent. |
39 | | - if (child_status == NodeStatus::FAILURE) |
40 | | - { |
41 | | - for (unsigned t = 0; t <= i; t++) |
| 36 | + case NodeStatus::RUNNING:{ |
| 37 | + return child_status; |
| 38 | + } |
| 39 | + case NodeStatus::FAILURE:{ |
| 40 | + for (unsigned t = 0; t <= index; t++) |
42 | 41 | { |
43 | 42 | children_nodes_[t]->setStatus(NodeStatus::IDLE); |
44 | 43 | } |
| 44 | + haltChildren(index + 1); |
| 45 | + return child_status; |
45 | 46 | } |
| 47 | + case NodeStatus::SUCCESS: { |
| 48 | + // continue; |
| 49 | + }break; |
46 | 50 |
|
47 | | - haltChildren(i + 1); |
48 | | - return child_status; |
49 | | - } |
50 | | - else |
51 | | - { |
52 | | - if (i == N_of_children - 1) |
53 | | - { |
54 | | - // If the child status is success, and it is the last child to be ticked, |
55 | | - // then the sequence has succeeded. |
56 | | - for (auto& ch : children_nodes_) |
57 | | - { |
58 | | - ch->setStatus(NodeStatus::IDLE); |
59 | | - } |
60 | | - return NodeStatus::SUCCESS; |
| 51 | + case NodeStatus::IDLE:{ |
| 52 | + throw std::runtime_error("This is not supposed to happen"); |
61 | 53 | } |
62 | | - } |
| 54 | + } // end switch |
| 55 | + }// end for loop |
| 56 | + |
| 57 | + for (auto& ch : children_nodes_) |
| 58 | + { |
| 59 | + ch->setStatus(NodeStatus::IDLE); |
63 | 60 | } |
64 | | - throw std::runtime_error("This is not supposed to happen"); |
| 61 | + return NodeStatus::SUCCESS; |
| 62 | +} |
| 63 | + |
65 | 64 | } |
0 commit comments