diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b9329074..db9490417 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,6 +112,7 @@ list(APPEND BT_SOURCE src/controls/if_then_else_node.cpp src/controls/fallback_node.cpp src/controls/parallel_node.cpp + src/controls/parallel_all_node.cpp src/controls/reactive_sequence.cpp src/controls/reactive_fallback.cpp src/controls/sequence_node.cpp diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index fa05e99ac..0760810b5 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -1,5 +1,5 @@ /* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved - * Copyright (C) 2018-2022 Davide Faconti - All Rights Reserved + * Copyright (C) 2018-2023 Davide Faconti - All Rights Reserved * * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, @@ -14,6 +14,7 @@ #pragma once #include "behaviortree_cpp/controls/parallel_node.h" +#include "behaviortree_cpp/controls/parallel_all_node.h" #include "behaviortree_cpp/controls/reactive_sequence.h" #include "behaviortree_cpp/controls/reactive_fallback.h" #include "behaviortree_cpp/controls/fallback_node.h" diff --git a/include/behaviortree_cpp/controls/parallel_all_node.h b/include/behaviortree_cpp/controls/parallel_all_node.h new file mode 100644 index 000000000..c0372a51b --- /dev/null +++ b/include/behaviortree_cpp/controls/parallel_all_node.h @@ -0,0 +1,62 @@ +/* Copyright (C) 2023 Davide Faconti - All Rights Reserved +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), +* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, +* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#pragma once + +#include +#include "behaviortree_cpp/control_node.h" + +namespace BT +{ +/** + * @brief The ParallelAllNode execute all its children + * __concurrently__, but not in separate threads! + * + * It differs in the way ParallelNode works because the latter may stop + * and halt other children if a certain number of SUCCESS/FAILURES is reached, + * whilst this one will always complete the execution of ALL its children. + * + * Note that threshold indexes work as in Python: + * https://www.i2tutorials.com/what-are-negative-indexes-and-why-are-they-used/ + * + * Therefore -1 is equivalent to the number of children. + */ +class ParallelAllNode : public ControlNode +{ +public: + + ParallelAllNode(const std::string& name, const NodeConfig& config); + + static PortsList providedPorts() + { + return {InputPort("max_failures", 1, + "If the number of children returning FAILURE exceeds this value, " + "ParallelAll returns FAILURE")}; + } + + ~ParallelAllNode() override = default; + + virtual void halt() override; + + size_t failureThreshold() const; + void setFailureThreshold(int threshold); + +private: + size_t failure_threshold_; + + std::set completed_list_; + size_t failure_count_ = 0; + + virtual BT::NodeStatus tick() override; +}; + +} // namespace BT diff --git a/include/behaviortree_cpp/controls/parallel_node.h b/include/behaviortree_cpp/controls/parallel_node.h index 9dc90aae9..fa2b042e0 100644 --- a/include/behaviortree_cpp/controls/parallel_node.h +++ b/include/behaviortree_cpp/controls/parallel_node.h @@ -65,15 +65,19 @@ class ParallelNode : public ControlNode private: int success_threshold_; int failure_threshold_; + + std::set completed_list_; - std::set skip_list_; - bool all_skipped_ = true; + size_t success_count_ = 0; + size_t failure_count_ = 0; bool read_parameter_from_ports_; static constexpr const char* THRESHOLD_SUCCESS = "success_count"; static constexpr const char* THRESHOLD_FAILURE = "failure_count"; virtual BT::NodeStatus tick() override; + + void clear(); }; } // namespace BT diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index dab0514f1..2f4d1cc4e 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -56,6 +56,7 @@ BehaviorTreeFactory::BehaviorTreeFactory(): #endif registerNodeType("Parallel"); + registerNodeType("ParallelAll"); registerNodeType("ReactiveSequence"); registerNodeType("ReactiveFallback"); registerNodeType("IfThenElse"); diff --git a/src/controls/parallel_all_node.cpp b/src/controls/parallel_all_node.cpp new file mode 100644 index 000000000..675428fb3 --- /dev/null +++ b/src/controls/parallel_all_node.cpp @@ -0,0 +1,131 @@ +/* Copyright (C) 2023 Davide Faconti - All Rights Reserved +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), +* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, +* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#include +#include + +#include "behaviortree_cpp/controls/parallel_all_node.h" + +namespace BT +{ + +ParallelAllNode::ParallelAllNode(const std::string& name, const NodeConfig& config) : + ControlNode::ControlNode(name, config), + failure_threshold_(1) +{} + +NodeStatus ParallelAllNode::tick() +{ + int max_failures = 0; + if (!getInput("max_failures", max_failures)) + { + throw RuntimeError("Missing parameter [max_failures] in ParallelNode"); + } + const size_t children_count = children_nodes_.size(); + setFailureThreshold(max_failures); + + size_t skipped_count = 0; + + if (children_count < failure_threshold_) + { + throw LogicError("Number of children is less than threshold. Can never fail."); + } + + setStatus(NodeStatus::RUNNING); + + // Routing the tree according to the sequence node's logic: + for (size_t index = 0; index < children_count; index++) + { + TreeNode* child_node = children_nodes_[index]; + + // already completed + if(completed_list_.count(index) != 0) + { + continue; + } + + NodeStatus const child_status = child_node->executeTick(); + + switch (child_status) + { + case NodeStatus::SUCCESS: { + completed_list_.insert(index); + } + break; + + case NodeStatus::FAILURE: { + completed_list_.insert(index); + failure_count_++; + } + break; + + case NodeStatus::RUNNING: { + // Still working. Check the next + } + break; + + case NodeStatus::SKIPPED: { + skipped_count++; + } + break; + + case NodeStatus::IDLE: { + throw LogicError("[", name(), "]: A children should not return IDLE"); + } + } + } + + if(skipped_count == children_count) + { + return NodeStatus::SKIPPED; + } + if( skipped_count + completed_list_.size() >= children_count) + { + // DONE + haltChildren(); + completed_list_.clear(); + failure_count_ = 0; + return (failure_count_ >= failure_threshold_) ? NodeStatus::FAILURE : + NodeStatus::SUCCESS; + } + + // Some children haven't finished, yet. + return NodeStatus::RUNNING; +} + +void ParallelAllNode::halt() +{ + completed_list_.clear(); + failure_count_ = 0; + ControlNode::halt(); +} + + +size_t ParallelAllNode::failureThreshold() const +{ + return failure_threshold_; +} + + +void ParallelAllNode::setFailureThreshold(int threshold) +{ + if (threshold < 0) + { + failure_threshold_ = size_t(std::max(int(children_nodes_.size()) + threshold + 1, 0)); + } + else + { + failure_threshold_ = size_t(threshold); + } +} + +} // namespace BT diff --git a/src/controls/parallel_node.cpp b/src/controls/parallel_node.cpp index 57006693f..0f3526be6 100644 --- a/src/controls/parallel_node.cpp +++ b/src/controls/parallel_node.cpp @@ -52,9 +52,6 @@ NodeStatus ParallelNode::tick() } } - size_t success_children_num = 0; - size_t failure_children_num = 0; - const size_t children_count = children_nodes_.size(); if (children_count < successThreshold()) @@ -67,79 +64,81 @@ NodeStatus ParallelNode::tick() throw LogicError("Number of children is less than threshold. Can never fail."); } - if(status() == NodeStatus::IDLE) - { - all_skipped_ = true; - } setStatus(NodeStatus::RUNNING); + size_t skipped_count = 0; + // Routing the tree according to the sequence node's logic: for (size_t i = 0; i < children_count; i++) { - TreeNode* child_node = children_nodes_[i]; - - bool const in_skip_list = (skip_list_.count(i) != 0); - - NodeStatus const prev_status = child_node->status(); - NodeStatus const child_status = - in_skip_list ? prev_status : child_node->executeTick(); + if(completed_list_.count(i) == 0) + { + TreeNode* child_node = children_nodes_[i]; + NodeStatus const child_status = child_node->executeTick(); + + switch (child_status) + { + case NodeStatus::SKIPPED: { + skipped_count++; + } break; + + case NodeStatus::SUCCESS: { + completed_list_.insert(i); + success_count_++; + } + break; - // switch to RUNNING state as soon as you find an active child - all_skipped_ &= (child_status == NodeStatus::SKIPPED); + case NodeStatus::FAILURE: { + completed_list_.insert(i); + failure_count_++; + } + break; - switch (child_status) - { - case NodeStatus::SUCCESS: { - skip_list_.insert(i); - success_children_num++; - - if (success_children_num == successThreshold()) - { - skip_list_.clear(); - resetChildren(); - return NodeStatus::SUCCESS; + case NodeStatus::RUNNING: { + // Still working. Check the next } - } - break; - - case NodeStatus::FAILURE: { - skip_list_.insert(i); - failure_children_num++; - - // It fails if it is not possible to succeed anymore or if - // number of failures are equal to failure_threshold_ - if ((failure_children_num > children_count - successThreshold()) || - (failure_children_num == failureThreshold())) - { - skip_list_.clear(); - resetChildren(); - return NodeStatus::FAILURE; + break; + + case NodeStatus::IDLE: { + throw LogicError("[", name(), "]: A children should not return IDLE"); } } - break; + } - case NodeStatus::RUNNING: { - // Still working. Check the next - } - break; + const size_t required_success_count = successThreshold(); - case NodeStatus::SKIPPED: { - // Node requested to be skipped or halted. Check the next - } - break; + if(success_count_ >= required_success_count || + (success_threshold_ < 0 && (success_count_ + skipped_count) >= required_success_count)) + { + clear(); + resetChildren(); + return NodeStatus::SUCCESS; + } - case NodeStatus::IDLE: { - throw LogicError("[", name(), "]: A children should not return IDLE"); - } + // It fails if it is not possible to succeed anymore or if + // number of failures are equal to failure_threshold_ + if (((children_count - failure_count_) < required_success_count) || + (failure_count_ == failureThreshold())) + { + clear(); + resetChildren(); + return NodeStatus::FAILURE; } } // Skip if ALL the nodes have been skipped - return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::RUNNING; + return (skipped_count == children_count) ? NodeStatus::SKIPPED : NodeStatus::RUNNING; +} + +void ParallelNode::clear() +{ + completed_list_.clear(); + success_count_ = 0; + failure_count_ = 0; } void ParallelNode::halt() { - skip_list_.clear(); + clear(); ControlNode::halt(); } diff --git a/src/tree_node.cpp b/src/tree_node.cpp index ddcd0dc56..aff050964 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -72,7 +72,7 @@ NodeStatus TreeNode::executeTick() { // injected pre-callback bool substituted = false; - if(_p->status == NodeStatus::IDLE) + if(!isStatusCompleted(_p->status)) { PreTickCallback callback; { @@ -181,7 +181,7 @@ Expected TreeNode::checkPreConditions() if (result.cast()) { // Some preconditions are applied only when the node is started - if (_p->status == NodeStatus::IDLE) + if (!isStatusCompleted(_p->status)) { if (preID == PreCond::FAILURE_IF) { diff --git a/tests/gtest_parallel.cpp b/tests/gtest_parallel.cpp index c405661b2..9148f3f6c 100644 --- a/tests/gtest_parallel.cpp +++ b/tests/gtest_parallel.cpp @@ -1,4 +1,5 @@ /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved +* Copyright (C) 2018-2023 Davide Faconti - All Rights Reserved * * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, @@ -15,6 +16,7 @@ #include "behaviortree_cpp/loggers/bt_observer.h" #include "condition_test_node.h" #include "behaviortree_cpp/bt_factory.h" +#include "test_helper.hpp" using BT::NodeStatus; using std::chrono::milliseconds; @@ -429,3 +431,30 @@ TEST(FailingParallel, FailingParallel) ASSERT_EQ(NodeStatus::SUCCESS, state); ASSERT_EQ( 1, observer.getStatistics("second").failure_count); } + +TEST(Parallel, Issue593) +{ + static const char* xml_text = R"( + + + +