diff --git a/src/controls/fallback_node.cpp b/src/controls/fallback_node.cpp index e27218e96..54b185b0a 100644 --- a/src/controls/fallback_node.cpp +++ b/src/controls/fallback_node.cpp @@ -17,8 +17,8 @@ namespace BT { FallbackNode::FallbackNode(const std::string& name) - : ControlNode::ControlNode(name, {} ) - ,current_child_idx_(0) + : ControlNode::ControlNode(name, {} ) + ,current_child_idx_(0) { setRegistrationID("Fallback"); } @@ -29,44 +29,42 @@ NodeStatus FallbackNode::tick() setStatus(NodeStatus::RUNNING); - while (current_child_idx_ < children_count) - { - TreeNode* current_child_node = children_nodes_[current_child_idx_]; - const NodeStatus child_status = current_child_node->executeTick(); - switch (child_status) + TreeNode* current_child_node = children_nodes_[current_child_idx_]; + const NodeStatus child_status = current_child_node->executeTick(); + + switch (child_status) + { + case NodeStatus::RUNNING: + { + return child_status; + } + case NodeStatus::SUCCESS: { - case NodeStatus::RUNNING: + haltChildren(0); + current_child_idx_ = 0; + return child_status; + } + case NodeStatus::FAILURE: + { + current_child_idx_++; + if( current_child_idx_ < children_count ) { - return child_status; + return NodeStatus::RUNNING; } - case NodeStatus::SUCCESS: - { + else{ haltChildren(0); current_child_idx_ = 0; - return child_status; - } - case NodeStatus::FAILURE: - { - current_child_idx_++; - } - break; - - case NodeStatus::IDLE: - { - throw LogicError("A child node must never return IDLE"); + return NodeStatus::FAILURE; } - } // end switch - } // end while loop + } - // The entire while loop completed. This means that all the children returned FAILURE. - if (current_child_idx_ == children_count) - { - haltChildren(0); - current_child_idx_ = 0; - } - - return NodeStatus::FAILURE; + case NodeStatus::IDLE: + { + throw LogicError("A child node must never return IDLE"); + } + } // end switch + // never reached } void FallbackNode::halt() diff --git a/src/controls/sequence_node.cpp b/src/controls/sequence_node.cpp index dfe63824b..ee2240613 100644 --- a/src/controls/sequence_node.cpp +++ b/src/controls/sequence_node.cpp @@ -19,7 +19,7 @@ namespace BT SequenceNode::SequenceNode(const std::string& name) - : ControlNode::ControlNode(name, {} ) + : ControlNode::ControlNode(name, {} ) , current_child_idx_(0) { setRegistrationID("Sequence"); @@ -37,44 +37,42 @@ NodeStatus SequenceNode::tick() setStatus(NodeStatus::RUNNING); - while (current_child_idx_ < children_count) - { - TreeNode* current_child_node = children_nodes_[current_child_idx_]; - const NodeStatus child_status = current_child_node->executeTick(); + TreeNode* current_child_node = children_nodes_[current_child_idx_]; + const NodeStatus child_status = current_child_node->executeTick(); - switch (child_status) + switch (child_status) + { + case NodeStatus::RUNNING: + { + return child_status; + } + case NodeStatus::FAILURE: { - case NodeStatus::RUNNING: + // Reset on failure + haltChildren(0); + current_child_idx_ = 0; + return child_status; + } + case NodeStatus::SUCCESS: + { + current_child_idx_++; + if( current_child_idx_ < children_count ) { - return child_status; + return NodeStatus::RUNNING; } - case NodeStatus::FAILURE: - { - // Reset on failure + else{ haltChildren(0); current_child_idx_ = 0; - return child_status; - } - case NodeStatus::SUCCESS: - { - current_child_idx_++; - } - break; - - case NodeStatus::IDLE: - { - throw LogicError("A child node must never return IDLE"); + return NodeStatus::SUCCESS; } - } // end switch - } // end while loop + } - // The entire while loop completed. This means that all the children returned SUCCESS. - if (current_child_idx_ == children_count) - { - haltChildren(0); - current_child_idx_ = 0; - } - return NodeStatus::SUCCESS; + case NodeStatus::IDLE: + { + throw LogicError("A child node must never return IDLE"); + } + } // end switch + // never reached } } diff --git a/src/controls/sequence_star_node.cpp b/src/controls/sequence_star_node.cpp index b588ce9db..51be8db19 100644 --- a/src/controls/sequence_star_node.cpp +++ b/src/controls/sequence_star_node.cpp @@ -17,7 +17,7 @@ namespace BT { SequenceStarNode::SequenceStarNode(const std::string& name) - : ControlNode::ControlNode(name, {} ) + : ControlNode::ControlNode(name, {} ) , current_child_idx_(0) { setRegistrationID("SequenceStar"); @@ -29,43 +29,41 @@ NodeStatus SequenceStarNode::tick() setStatus(NodeStatus::RUNNING); - while (current_child_idx_ < children_count) - { - TreeNode* current_child_node = children_nodes_[current_child_idx_]; - const NodeStatus child_status = current_child_node->executeTick(); + TreeNode* current_child_node = children_nodes_[current_child_idx_]; + const NodeStatus child_status = current_child_node->executeTick(); - switch (child_status) + switch (child_status) + { + case NodeStatus::RUNNING: { - case NodeStatus::RUNNING: - { - return child_status; - } - case NodeStatus::FAILURE: - { - // DO NOT reset on failure - haltChildren(current_child_idx_); - return child_status; - } - case NodeStatus::SUCCESS: + return child_status; + } + case NodeStatus::FAILURE: + { + // DO NOT reset on failure + haltChildren(current_child_idx_); + return child_status; + } + case NodeStatus::SUCCESS: + { + current_child_idx_++; + if( current_child_idx_ < children_count ) { - current_child_idx_++; + return NodeStatus::RUNNING; } - break; - - case NodeStatus::IDLE: - { - throw LogicError("A child node must never return IDLE"); + else{ + haltChildren(0); + current_child_idx_ = 0; + return NodeStatus::SUCCESS; } - } // end switch - } // end while loop + } - // The entire while loop completed. This means that all the children returned SUCCESS. - if (current_child_idx_ == children_count) - { - haltChildren(0); - current_child_idx_ = 0; - } - return NodeStatus::SUCCESS; + case NodeStatus::IDLE: + { + throw LogicError("A child node must never return IDLE"); + } + } // end switch + // never reached } void SequenceStarNode::halt() diff --git a/src/decorators/repeat_node.cpp b/src/decorators/repeat_node.cpp index fa5161e8a..1a66a4871 100644 --- a/src/decorators/repeat_node.cpp +++ b/src/decorators/repeat_node.cpp @@ -18,19 +18,19 @@ namespace BT constexpr const char* RepeatNode::NUM_CYCLES; RepeatNode::RepeatNode(const std::string& name, unsigned int NTries) - : DecoratorNode(name, {} ), - num_cycles_(NTries), - try_index_(0), - read_parameter_from_ports_(false) + : DecoratorNode(name, {} ), + num_cycles_(NTries), + try_index_(0), + read_parameter_from_ports_(false) { - setRegistrationID("Repeat"); + setRegistrationID("Repeat"); } RepeatNode::RepeatNode(const std::string& name, const NodeConfiguration& config) : DecoratorNode(name, config), - num_cycles_(0), - try_index_(0), - read_parameter_from_ports_(true) + num_cycles_(0), + try_index_(0), + read_parameter_from_ports_(true) { } @@ -47,38 +47,40 @@ NodeStatus RepeatNode::tick() setStatus(NodeStatus::RUNNING); - while (try_index_ < num_cycles_) - { - NodeStatus child_state = child_node_->executeTick(); + NodeStatus child_state = child_node_->executeTick(); - switch (child_state) + switch (child_state) + { + case NodeStatus::SUCCESS: { - case NodeStatus::SUCCESS: + try_index_++; + if( try_index_ < num_cycles_) { - try_index_++; + return NodeStatus::RUNNING; } - break; - - case NodeStatus::FAILURE: - { + else{ try_index_ = 0; - return (NodeStatus::FAILURE); + return NodeStatus::SUCCESS; } + } - case NodeStatus::RUNNING: - { - return NodeStatus::RUNNING; - } + case NodeStatus::FAILURE: + { + try_index_ = 0; + return (NodeStatus::FAILURE); + } - default: - { - throw LogicError("A child node must never return IDLE"); - } + case NodeStatus::RUNNING: + { + return NodeStatus::RUNNING; } - } - try_index_ = 0; - return NodeStatus::SUCCESS; + default: + { + throw LogicError("A child node must never return IDLE"); + } + } + // never reached } void RepeatNode::halt() diff --git a/src/decorators/retry_node.cpp b/src/decorators/retry_node.cpp index 75bd28613..e97d4c4c6 100644 --- a/src/decorators/retry_node.cpp +++ b/src/decorators/retry_node.cpp @@ -18,19 +18,19 @@ namespace BT constexpr const char* RetryNode::NUM_ATTEMPTS; RetryNode::RetryNode(const std::string& name, unsigned int NTries) - : DecoratorNode(name, {} ), - max_attempts_(NTries), - try_index_(0), - read_parameter_from_ports_(false) + : DecoratorNode(name, {} ), + max_attempts_(NTries), + try_index_(0), + read_parameter_from_ports_(false) { setRegistrationID("RetryUntilSuccesful"); } RetryNode::RetryNode(const std::string& name, const NodeConfiguration& config) : DecoratorNode(name, config), - max_attempts_(0), - try_index_(0), - read_parameter_from_ports_(true) + max_attempts_(0), + try_index_(0), + read_parameter_from_ports_(true) { } @@ -52,38 +52,40 @@ NodeStatus RetryNode::tick() setStatus(NodeStatus::RUNNING); - while (try_index_ < max_attempts_) + NodeStatus child_state = child_node_->executeTick(); + + switch (child_state) { - NodeStatus child_state = child_node_->executeTick(); + case NodeStatus::SUCCESS: + { + try_index_ = 0; + return (NodeStatus::SUCCESS); + } - switch (child_state) + case NodeStatus::FAILURE: { - case NodeStatus::SUCCESS: + try_index_++; + if( try_index_ < max_attempts_) { - try_index_ = 0; - return (NodeStatus::SUCCESS); + return NodeStatus::RUNNING; } - - case NodeStatus::FAILURE: - { - try_index_++; + else{ + try_index_ = 0; + return NodeStatus::FAILURE; } - break; + } - case NodeStatus::RUNNING: - { - return NodeStatus::RUNNING; - } + case NodeStatus::RUNNING: + { + return NodeStatus::RUNNING; + } - default: - { - throw LogicError("A child node must never return IDLE"); - } + default: + { + throw LogicError("A child node must never return IDLE"); } } - - try_index_ = 0; - return NodeStatus::FAILURE; + // never reached } }