threshold child count dynamically in parallel control node (#363)

Author: zflat

include/behaviortree_cpp_v3/controls/parallel_node.h

@@ -41,12 +41,12 @@ class ParallelNode : public ControlNode
 
     unsigned int thresholdM();
     unsigned int thresholdFM();
-    void setThresholdM(unsigned int threshold_M);
-    void setThresholdFM(unsigned int threshold_M);
+    void setThresholdM(int threshold_M);
+    void setThresholdFM(int threshold_M);
 
   private:
-    unsigned int success_threshold_;
-    unsigned int failure_threshold_;
+    int success_threshold_;
+    int failure_threshold_;
 
     std::set<int> skip_list_;
 

src/controls/parallel_node.cpp

@@ -11,6 +11,9 @@
 *   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_v3/controls/parallel_node.h"
 
 namespace BT
@@ -58,12 +61,12 @@ NodeStatus ParallelNode::tick()
 
     const size_t children_count = children_nodes_.size();
 
-    if( children_count < success_threshold_)
+    if( children_count < thresholdM())
     {
         throw LogicError("Number of children is less than threshold. Can never succeed.");
     }
 
-    if( children_count < failure_threshold_)
+    if( children_count < thresholdFM())
     {
         throw LogicError("Number of children is less than threshold. Can never fail.");
     }
@@ -94,7 +97,7 @@ NodeStatus ParallelNode::tick()
                 }
                 success_childred_num++;
 
-                if (success_childred_num == success_threshold_)
+                if (success_childred_num == thresholdM())
                 {
                     skip_list_.clear();
                     haltChildren();
@@ -112,8 +115,8 @@ NodeStatus ParallelNode::tick()
 
                 // It fails if it is not possible to succeed anymore or if 
                 // number of failures are equal to failure_threshold_
-                if ((failure_childred_num > children_count - success_threshold_)
-                    || (failure_childred_num == failure_threshold_))
+                if ((failure_childred_num > children_count - thresholdM())
+                    || (failure_childred_num == thresholdFM()))
                 {
                     skip_list_.clear();
                     haltChildren();
@@ -144,20 +147,24 @@ void ParallelNode::halt()
 
 unsigned int ParallelNode::thresholdM()
 {
-    return success_threshold_;
+    return success_threshold_ < 0
+      ? std::max(children_nodes_.size() + success_threshold_ + 1, static_cast<std::size_t>(0))
+      : success_threshold_;
 }
 
 unsigned int ParallelNode::thresholdFM()
 {
-    return failure_threshold_;
+    return failure_threshold_ < 0
+      ? std::max(children_nodes_.size() + failure_threshold_ + 1, static_cast<std::size_t>(0))
+      : failure_threshold_;
 }
 
-void ParallelNode::setThresholdM(unsigned int threshold_M)
+void ParallelNode::setThresholdM(int threshold_M)
 {
     success_threshold_ = threshold_M;
 }
 
-void ParallelNode::setThresholdFM(unsigned int threshold_M)
+void ParallelNode::setThresholdFM(int threshold_M)
 {
     failure_threshold_ = threshold_M;
 }

tests/gtest_parallel.cpp

@@ -145,6 +145,85 @@ TEST_F(SimpleParallelTest, Threshold_3)
     ASSERT_EQ(NodeStatus::SUCCESS, state);
 }
 
+TEST_F(SimpleParallelTest, Threshold_neg2)
+{
+    root.setThresholdM(-2);
+    action_1.setTime( milliseconds(100) );
+    action_2.setTime( milliseconds(500) ); // this takes a lot of time
+
+    BT::NodeStatus state = root.executeTick();
+    // first tick, zero wait
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for( milliseconds(150) );
+    state = root.executeTick();
+    // second tick: action1 should be completed, but not action2
+    // nevertheless it is sufficient because threshold is 3
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+
+TEST_F(SimpleParallelTest, Threshold_neg1)
+{
+    root.setThresholdM(-1);
+    action_1.setTime( milliseconds(100) );
+    action_2.setTime( milliseconds(500) ); // this takes a lot of time
+
+    BT::NodeStatus state = root.executeTick();
+    // first tick, zero wait
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for( milliseconds(150) );
+    state = root.executeTick();
+    // second tick: action1 should be completed, but not action2
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for( milliseconds(650) );
+    state = root.executeTick();
+    // third tick: all actions completed
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+
+TEST_F(SimpleParallelTest, Threshold_thresholdFneg1)
+{
+    root.setThresholdM(1);
+    root.setThresholdFM(-1);
+    action_1.setTime( milliseconds(100) );
+    action_1.setExpectedResult(NodeStatus::FAILURE);
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    action_2.setTime( milliseconds(200) );
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+    action_2.setExpectedResult(NodeStatus::FAILURE);
+
+    BT::NodeStatus state = root.executeTick();
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(250));
+    state = root.executeTick();
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+}
+
 TEST_F(SimpleParallelTest, Threshold_2)
 {
     root.setThresholdM(2);
@@ -271,6 +350,7 @@ TEST_F(ComplexParallelTest, ConditionRightFalse_thresholdF_2)
     ASSERT_EQ(NodeStatus::SUCCESS, state);
 }
 
+
 TEST_F(ComplexParallelTest, ConditionRightFalseAction1Done)
 {
     condition_R.setExpectedResult(NodeStatus::FAILURE);