Merge branch 'parallel_all'

Author: facontidavide

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

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"

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 <set>
+#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<int>("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<size_t> completed_list_;
+  size_t failure_count_ = 0;
+
+  virtual BT::NodeStatus tick() override;
+};
+
+}   // namespace BT

src/bt_factory.cpp

@@ -56,6 +56,7 @@ BehaviorTreeFactory::BehaviorTreeFactory():
 #endif
 
   registerNodeType<ParallelNode>("Parallel");
+  registerNodeType<ParallelAllNode>("ParallelAll");
   registerNodeType<ReactiveSequence>("ReactiveSequence");
   registerNodeType<ReactiveFallback>("ReactiveFallback");
   registerNodeType<IfThenElseNode>("IfThenElse");
@@ -632,7 +633,7 @@ NodeStatus Tree::tickRoot(TickOption opt, std::chrono::milliseconds sleep_time)
       status = rootNode()->executeTick();
     }
 
-    if (status == NodeStatus::SUCCESS || status == NodeStatus::FAILURE)
+    if (isStatusCompleted(status))
     {
       rootNode()->resetStatus();
     }

src/controls/parallel_all_node.cpp

@@ -0,0 +1,132 @@
+/* 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 <algorithm>
+#include <cstddef>
+
+#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();
+    auto const status = (failure_count_ >= failure_threshold_) ?
+                            NodeStatus::FAILURE : NodeStatus::SUCCESS;
+    failure_count_ = 0;
+    return status;
+  }
+
+  // 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

tests/gtest_parallel.cpp

@@ -395,15 +395,15 @@ TEST_F(ComplexParallelTest, ConditionRightFalseAction1Done)
   ASSERT_EQ(NodeStatus::SUCCESS, state);
 }
 
-TEST(FailingParallel, FailingParallel)
+TEST(Parallel, FailingParallel)
 {
   static const char* xml_text = R"(
 <root BTCPP_format="4">
   <BehaviorTree ID="MainTree">
     <Parallel name="parallel" success_count="1" failure_count="3">
       <GoodTest name="first"/>
       <BadTest name="second"/>
-      <GoodTest name="third"/>
+      <SlowTest name="third"/>
     </Parallel>
   </BehaviorTree>
 </root>  )";
@@ -412,7 +412,7 @@ TEST(FailingParallel, FailingParallel)
   BehaviorTreeFactory factory;
 
   BT::TestNodeConfig good_config;
-  good_config.async_delay = std::chrono::milliseconds(300);
+  good_config.async_delay = std::chrono::milliseconds(200);
   good_config.return_status = NodeStatus::SUCCESS;
   factory.registerNodeType<BT::TestNode>("GoodTest", good_config);
 
@@ -421,11 +421,77 @@ TEST(FailingParallel, FailingParallel)
   bad_config.return_status = NodeStatus::FAILURE;
   factory.registerNodeType<BT::TestNode>("BadTest", bad_config);
 
+  BT::TestNodeConfig slow_config;
+  slow_config.async_delay = std::chrono::milliseconds(300);
+  slow_config.return_status = NodeStatus::SUCCESS;
+  factory.registerNodeType<BT::TestNode>("SlowTest", slow_config);
+
   auto tree = factory.createTreeFromText(xml_text);
   BT::TreeObserver observer(tree);
 
   auto state = tree.tickWhileRunning();
   // since at least one succeeded.
   ASSERT_EQ(NodeStatus::SUCCESS, state);
+  ASSERT_EQ( 1, observer.getStatistics("first").success_count);
   ASSERT_EQ( 1, observer.getStatistics("second").failure_count);
+  ASSERT_EQ( 0, observer.getStatistics("third").failure_count);
+}
+
+TEST(Parallel, ParallelAll)
+{
+  using namespace BT;
+
+  BehaviorTreeFactory factory;
+
+  BT::TestNodeConfig good_config;
+  good_config.async_delay = std::chrono::milliseconds(300);
+  good_config.return_status = NodeStatus::SUCCESS;
+  factory.registerNodeType<BT::TestNode>("GoodTest", good_config);
+
+  BT::TestNodeConfig bad_config;
+  bad_config.async_delay = std::chrono::milliseconds(100);
+  bad_config.return_status = NodeStatus::FAILURE;
+  factory.registerNodeType<BT::TestNode>("BadTest", bad_config);
+
+  {
+    const char* xml_text = R"(
+<root BTCPP_format="4">
+  <BehaviorTree ID="MainTree">
+    <ParallelAll max_failures="1">
+      <BadTest name="first"/>
+      <GoodTest name="second"/>
+      <GoodTest name="third"/>
+    </ParallelAll>
+  </BehaviorTree>
+</root>  )";
+    auto tree = factory.createTreeFromText(xml_text);
+    BT::TreeObserver observer(tree);
+
+    auto state = tree.tickWhileRunning();
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+    ASSERT_EQ( 1, observer.getStatistics("first").failure_count);
+    ASSERT_EQ( 1, observer.getStatistics("second").success_count);
+    ASSERT_EQ( 1, observer.getStatistics("third").success_count);
+  }
+
+  {
+    const char* xml_text = R"(
+<root BTCPP_format="4">
+  <BehaviorTree ID="MainTree">
+    <ParallelAll max_failures="2">
+      <BadTest name="first"/>
+      <GoodTest name="second"/>
+      <GoodTest name="third"/>
+    </ParallelAll>
+  </BehaviorTree>
+</root>  )";
+    auto tree = factory.createTreeFromText(xml_text);
+    BT::TreeObserver observer(tree);
+
+    auto state = tree.tickWhileRunning();
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+    ASSERT_EQ( 1, observer.getStatistics("first").failure_count);
+    ASSERT_EQ( 1, observer.getStatistics("second").success_count);
+    ASSERT_EQ( 1, observer.getStatistics("third").success_count);
+  }
 }