adding ParallelAll, WIP

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");

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 <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();
+    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