diff --git a/include/behaviortree_cpp_v3/control_node.h b/include/behaviortree_cpp_v3/control_node.h index 3f8de1dde..306a05cba 100644 --- a/include/behaviortree_cpp_v3/control_node.h +++ b/include/behaviortree_cpp_v3/control_node.h @@ -42,6 +42,7 @@ class ControlNode : public TreeNode virtual void halt() override; + /// same as resetChildren() void haltChildren(); [[deprecated("deprecated: please use explicitly haltChildren() or haltChild(i)")]] void @@ -53,5 +54,9 @@ class ControlNode : public TreeNode { return NodeType::CONTROL; } + + /// Set the status of all children to IDLE. + /// also send a halt() signal to all RUNNING children + void resetChildren(); }; } // namespace BT diff --git a/include/behaviortree_cpp_v3/controls/switch_node.h b/include/behaviortree_cpp_v3/controls/switch_node.h index 30b341594..596087fc7 100644 --- a/include/behaviortree_cpp_v3/controls/switch_node.h +++ b/include/behaviortree_cpp_v3/controls/switch_node.h @@ -119,7 +119,7 @@ inline NodeStatus SwitchNode::tick() } else { - haltChildren(); + resetChildren(); running_child_ = -1; } return ret; diff --git a/include/behaviortree_cpp_v3/decorator_node.h b/include/behaviortree_cpp_v3/decorator_node.h index 546d62d7b..3b40dabc9 100644 --- a/include/behaviortree_cpp_v3/decorator_node.h +++ b/include/behaviortree_cpp_v3/decorator_node.h @@ -24,7 +24,7 @@ class DecoratorNode : public TreeNode /// The method used to interrupt the execution of this node virtual void halt() override; - /// Halt() the child node + /// Same as resetChild() void haltChild(); virtual NodeType type() const override @@ -33,6 +33,10 @@ class DecoratorNode : public TreeNode } NodeStatus executeTick() override; + + /// Set the status of the child to IDLE. + /// also send a halt() signal to a RUNNING child + void resetChild(); }; /** diff --git a/src/control_node.cpp b/src/control_node.cpp index a25a2bdcb..4ec6a79fc 100644 --- a/src/control_node.cpp +++ b/src/control_node.cpp @@ -31,7 +31,19 @@ size_t ControlNode::childrenCount() const void ControlNode::halt() { - haltChildren(); + resetChildren(); +} + +void ControlNode::resetChildren() +{ + for (auto child: children_nodes_) + { + if (child->status() == NodeStatus::RUNNING) + { + child->halt(); + } + child->resetStatus(); + } } const std::vector& ControlNode::children() const diff --git a/src/controls/fallback_node.cpp b/src/controls/fallback_node.cpp index d7fb1df54..fc580d4f7 100644 --- a/src/controls/fallback_node.cpp +++ b/src/controls/fallback_node.cpp @@ -38,7 +38,7 @@ NodeStatus FallbackNode::tick() return child_status; } case NodeStatus::SUCCESS: { - haltChildren(); + resetChildren(); current_child_idx_ = 0; return child_status; } @@ -56,7 +56,7 @@ NodeStatus FallbackNode::tick() // The entire while loop completed. This means that all the children returned FAILURE. if (current_child_idx_ == children_count) { - haltChildren(); + resetChildren(); current_child_idx_ = 0; } diff --git a/src/controls/if_then_else_node.cpp b/src/controls/if_then_else_node.cpp index 7582f8de0..a5786ccae 100644 --- a/src/controls/if_then_else_node.cpp +++ b/src/controls/if_then_else_node.cpp @@ -71,7 +71,7 @@ NodeStatus IfThenElseNode::tick() } else { - haltChildren(); + resetChildren(); child_idx_ = 0; return status; } diff --git a/src/controls/parallel_node.cpp b/src/controls/parallel_node.cpp index 78ab08a16..bf1604388 100644 --- a/src/controls/parallel_node.cpp +++ b/src/controls/parallel_node.cpp @@ -97,7 +97,7 @@ NodeStatus ParallelNode::tick() if (success_childred_num == successThreshold()) { skip_list_.clear(); - haltChildren(); + resetChildren(); return NodeStatus::SUCCESS; } } @@ -116,7 +116,7 @@ NodeStatus ParallelNode::tick() (failure_childred_num == failureThreshold())) { skip_list_.clear(); - haltChildren(); + resetChildren(); return NodeStatus::FAILURE; } } diff --git a/src/controls/reactive_fallback.cpp b/src/controls/reactive_fallback.cpp index c89d43e00..afab1144c 100644 --- a/src/controls/reactive_fallback.cpp +++ b/src/controls/reactive_fallback.cpp @@ -39,7 +39,7 @@ NodeStatus ReactiveFallback::tick() break; case NodeStatus::SUCCESS: { - haltChildren(); + resetChildren(); return NodeStatus::SUCCESS; } @@ -51,7 +51,7 @@ NodeStatus ReactiveFallback::tick() if (failure_count == childrenCount()) { - haltChildren(); + resetChildren(); return NodeStatus::FAILURE; } diff --git a/src/controls/reactive_sequence.cpp b/src/controls/reactive_sequence.cpp index 2e1d81289..efb843a0f 100644 --- a/src/controls/reactive_sequence.cpp +++ b/src/controls/reactive_sequence.cpp @@ -37,7 +37,7 @@ NodeStatus ReactiveSequence::tick() } case NodeStatus::FAILURE: { - haltChildren(); + resetChildren(); return NodeStatus::FAILURE; } case NodeStatus::SUCCESS: { @@ -53,7 +53,7 @@ NodeStatus ReactiveSequence::tick() if (success_count == childrenCount()) { - haltChildren(); + resetChildren(); return NodeStatus::SUCCESS; } return NodeStatus::RUNNING; diff --git a/src/controls/sequence_node.cpp b/src/controls/sequence_node.cpp index 11a224d51..9de146418 100644 --- a/src/controls/sequence_node.cpp +++ b/src/controls/sequence_node.cpp @@ -46,7 +46,7 @@ NodeStatus SequenceNode::tick() } case NodeStatus::FAILURE: { // Reset on failure - haltChildren(); + resetChildren(); current_child_idx_ = 0; return child_status; } @@ -64,7 +64,7 @@ NodeStatus SequenceNode::tick() // The entire while loop completed. This means that all the children returned SUCCESS. if (current_child_idx_ == children_count) { - haltChildren(); + resetChildren(); current_child_idx_ = 0; } return NodeStatus::SUCCESS; diff --git a/src/controls/sequence_star_node.cpp b/src/controls/sequence_star_node.cpp index a697278ed..bdd65c358 100644 --- a/src/controls/sequence_star_node.cpp +++ b/src/controls/sequence_star_node.cpp @@ -60,7 +60,7 @@ NodeStatus SequenceStarNode::tick() // The entire while loop completed. This means that all the children returned SUCCESS. if (current_child_idx_ == children_count) { - haltChildren(); + resetChildren(); current_child_idx_ = 0; } return NodeStatus::SUCCESS; diff --git a/src/controls/while_do_else_node.cpp b/src/controls/while_do_else_node.cpp index 543eefae4..b0ecae3cc 100644 --- a/src/controls/while_do_else_node.cpp +++ b/src/controls/while_do_else_node.cpp @@ -62,7 +62,7 @@ NodeStatus WhileDoElseNode::tick() } else { - haltChildren(); + resetChildren(); return status; } } diff --git a/src/decorator_node.cpp b/src/decorator_node.cpp index c239e0dbd..263e16bf0 100644 --- a/src/decorator_node.cpp +++ b/src/decorator_node.cpp @@ -31,7 +31,7 @@ void DecoratorNode::setChild(TreeNode* child) void DecoratorNode::halt() { - haltChild(); + resetChild(); } const TreeNode* DecoratorNode::child() const @@ -45,6 +45,11 @@ TreeNode* DecoratorNode::child() } void DecoratorNode::haltChild() +{ + resetChild(); +} + +void DecoratorNode::resetChild() { if (!child_node_) { @@ -57,6 +62,7 @@ void DecoratorNode::haltChild() child_node_->resetStatus(); } + SimpleDecoratorNode::SimpleDecoratorNode(const std::string& name, TickFunctor tick_functor, const NodeConfiguration& config) : diff --git a/src/decorators/delay_node.cpp b/src/decorators/delay_node.cpp index a5962f231..daed76039 100644 --- a/src/decorators/delay_node.cpp +++ b/src/decorators/delay_node.cpp @@ -68,6 +68,7 @@ NodeStatus DelayNode::tick() { delay_started_ = false; delay_aborted_ = false; + resetChild(); } return child_status; } diff --git a/src/decorators/inverter_node.cpp b/src/decorators/inverter_node.cpp index caf60c815..5e325ff20 100644 --- a/src/decorators/inverter_node.cpp +++ b/src/decorators/inverter_node.cpp @@ -29,10 +29,12 @@ NodeStatus InverterNode::tick() switch (child_state) { case NodeStatus::SUCCESS: { + resetChild(); return NodeStatus::FAILURE; } case NodeStatus::FAILURE: { + resetChild(); return NodeStatus::SUCCESS; } diff --git a/src/decorators/repeat_node.cpp b/src/decorators/repeat_node.cpp index 1aa0d4a31..1925765ca 100644 --- a/src/decorators/repeat_node.cpp +++ b/src/decorators/repeat_node.cpp @@ -53,13 +53,13 @@ NodeStatus RepeatNode::tick() { case NodeStatus::SUCCESS: { repeat_count_++; - haltChild(); + resetChild(); } break; case NodeStatus::FAILURE: { repeat_count_ = 0; - haltChild(); + resetChild(); return (NodeStatus::FAILURE); } diff --git a/src/decorators/retry_node.cpp b/src/decorators/retry_node.cpp index 7cea25487..3fb2e4740 100644 --- a/src/decorators/retry_node.cpp +++ b/src/decorators/retry_node.cpp @@ -58,13 +58,13 @@ NodeStatus RetryNode::tick() { case NodeStatus::SUCCESS: { try_count_ = 0; - haltChild(); + resetChild(); return (NodeStatus::SUCCESS); } case NodeStatus::FAILURE: { try_count_++; - haltChild(); + resetChild(); } break; diff --git a/src/decorators/subtree_node.cpp b/src/decorators/subtree_node.cpp index b3bb257f7..fa8469f40 100644 --- a/src/decorators/subtree_node.cpp +++ b/src/decorators/subtree_node.cpp @@ -12,7 +12,13 @@ BT::NodeStatus BT::SubtreeNode::tick() { setStatus(NodeStatus::RUNNING); } - return child_node_->executeTick(); + auto status = child_node_->executeTick(); + if(status != NodeStatus::RUNNING) + { + resetChild(); + } + + return status; } //--------------------------------