Merge branch 'master' of github.com:BehaviorTree/BehaviorTree.CPP

Author: facontidavide

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

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<NodeStatus> TreeNode::checkPreConditions()
     if (result.cast<bool>())
     {
       // Some preconditions are applied only when the node is started
-      if (_p->status == NodeStatus::IDLE)
+      if (!isStatusCompleted(_p->status))
       {
         if (preID == PreCond::FAILURE_IF)
         {

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;
@@ -495,3 +497,30 @@ TEST(Parallel, ParallelAll)
     ASSERT_EQ( 1, observer.getStatistics("third").success_count);
   }
 }
+
+TEST(Parallel, Issue593)
+{
+  static const char* xml_text = R"(
+<root BTCPP_format="4">
+  <BehaviorTree ID="TestTree">
+    <Sequence>
+      <Script code="test := true"/>
+      <Parallel failure_count="1" success_count="-1">
+        <TestA _skipIf="test == true"/>
+        <Sleep msec="100"/>
+      </Parallel>
+    </Sequence>
+  </BehaviorTree>
+</root>
+)";
+  using namespace BT;
+
+  BehaviorTreeFactory factory;
+  std::array<int, 1> counters;
+  RegisterTestTick(factory, "Test", counters);
+
+  auto tree = factory.createTreeFromText(xml_text);
+  tree.tickWhileRunning();
+
+  ASSERT_EQ(0, counters[0]);
+}