Fix Issue 593 (#594): support skipping in Parallel node

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

include/behaviortree_cpp/controls/parallel_node.h

@@ -65,15 +65,19 @@ class ParallelNode : public ControlNode
 private:
   int success_threshold_;
   int failure_threshold_;
+  
+  std::set<size_t> completed_list_;
 
-  std::set<size_t> 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

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

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