fix ReactiveFallback too

Davide Faconti · Davide Faconti · commit b58b80212eaa · 2019-04-03T10:38:53.000+02:00
diff --git a/include/behaviortree_cpp/controls/reactive_fallback.h b/include/behaviortree_cpp/controls/reactive_fallback.h
@@ -39,9 +39,12 @@ class ReactiveFallback : public ControlNode
     ReactiveFallback(const std::string& name):
       ControlNode(name, {}){}
 
+    virtual void halt() override;
+
   private:
 
     virtual BT::NodeStatus tick() override;
+    std::vector<bool> is_asynch_child_;
 };
 
 }
diff --git a/src/controls/reactive_fallback.cpp b/src/controls/reactive_fallback.cpp
@@ -15,21 +15,35 @@
 namespace BT
 {
 
+void ReactiveFallback::halt()
+{
+    std::fill(is_asynch_child_.begin(), is_asynch_child_.end(), false );
+    ControlNode::halt();
+}
+
 NodeStatus ReactiveFallback::tick()
 {
+    is_asynch_child_.resize(childrenCount(), false);
     size_t failure_count = 0;
-    size_t running_count = 0;
 
     for (size_t index = 0; index < childrenCount(); index++)
     {
         TreeNode* current_child_node = children_nodes_[index];
         const NodeStatus child_status = current_child_node->executeTick();
 
+        if( is_asynch_child_[index] &&
+            current_child_node->status() == NodeStatus::FAILURE )
+        {
+            failure_count++;
+            continue; // skip already executed asynch children
+        }
+
         switch (child_status)
         {
             case NodeStatus::RUNNING:
             {
-                haltChildren(0);
+                is_asynch_child_[index] = true;
+                haltChildren(index+1);
                 return NodeStatus::RUNNING;
             }
 
@@ -40,7 +54,7 @@ NodeStatus ReactiveFallback::tick()
 
             case NodeStatus::SUCCESS:
             {
-                haltChildren(0);
+                halt();
                 return NodeStatus::SUCCESS;
             }
 
@@ -51,13 +65,12 @@ NodeStatus ReactiveFallback::tick()
         }   // end switch
     } //end for
 
-    if( failure_count == childrenCount() )
+    if( failure_count != childrenCount() )
     {
-        haltChildren(0);
-        return NodeStatus::FAILURE;
+        throw LogicError("This is not supposed to happen");
     }
-
-    return NodeStatus::RUNNING;
+    halt();
+    return NodeStatus::FAILURE;
 }
 
 }
diff --git a/src/controls/reactive_sequence.cpp b/src/controls/reactive_sequence.cpp
@@ -25,12 +25,12 @@ NodeStatus ReactiveSequence::tick()
 {
     is_asynch_child_.resize(childrenCount(), false);
     size_t success_count = 0;
-    size_t running_count = 0;
 
     for (size_t index = 0; index < childrenCount(); index++)
     {
         TreeNode* current_child_node = children_nodes_[index];
-        if( is_asynch_child_[index] && current_child_node->status() == NodeStatus::SUCCESS )
+        if( is_asynch_child_[index] &&
+            current_child_node->status() == NodeStatus::SUCCESS )
         {
             success_count++;
             continue; // skip already executed asynch children
@@ -43,7 +43,6 @@ NodeStatus ReactiveSequence::tick()
             case NodeStatus::RUNNING:
             {
                 is_asynch_child_[index] = true;
-                running_count++;
                 haltChildren(index+1);
                 return NodeStatus::RUNNING;
             }
@@ -65,12 +64,12 @@ NodeStatus ReactiveSequence::tick()
         }   // end switch
     } //end for
 
-    if( success_count == childrenCount())
+    if( success_count != childrenCount())
     {
-        halt();
-        return NodeStatus::SUCCESS;
+        throw LogicError("This is not supposed to happen");
     }
-    return NodeStatus::RUNNING;
+    halt();
+    return NodeStatus::SUCCESS;
 }
 
 
diff --git a/tests/gtest_sequence.cpp b/tests/gtest_sequence.cpp
@@ -415,7 +415,6 @@ TEST_F(ReactiveSequenceTest, ReactiveSequence)
     ASSERT_EQ(NodeStatus::FAILURE, state);
     ASSERT_GE( elapsed_ms, 100 );
     ASSERT_LE( elapsed_ms, 120 );
-
 }
 
 

Original file line number	Diff line number	Diff line change
`@@ -15,21 +15,35 @@`
`15`	`15`	`namespace BT`
`16`	`16`	`{`
`17`	`17`
	`18`	`+void ReactiveFallback::halt()`
	`19`	`+{`
	`20`	`+ std::fill(is_asynch_child_.begin(), is_asynch_child_.end(), false );`
	`21`	`+ ControlNode::halt();`
	`22`	`+}`
	`23`	`+`
`18`	`24`	`NodeStatus ReactiveFallback::tick()`
`19`	`25`	`{`
	`26`	`+ is_asynch_child_.resize(childrenCount(), false);`
`20`	`27`	`size_t failure_count = 0;`
`21`		`- size_t running_count = 0;`
`22`	`28`
`23`	`29`	`for (size_t index = 0; index < childrenCount(); index++)`
`24`	`30`	`{`
`25`	`31`	`TreeNode* current_child_node = children_nodes_[index];`
`26`	`32`	`const NodeStatus child_status = current_child_node->executeTick();`
`27`	`33`
	`34`	`+ if( is_asynch_child_[index] &&`
	`35`	`+ current_child_node->status() == NodeStatus::FAILURE )`
	`36`	`+ {`
	`37`	`+ failure_count++;`
	`38`	`+ continue; // skip already executed asynch children`
	`39`	`+ }`
	`40`	`+`
`28`	`41`	`switch (child_status)`
`29`	`42`	`{`
`30`	`43`	`case NodeStatus::RUNNING:`
`31`	`44`	`{`
`32`		`- haltChildren(0);`
	`45`	`+ is_asynch_child_[index] = true;`
	`46`	`+ haltChildren(index+1);`
`33`	`47`	`return NodeStatus::RUNNING;`
`34`	`48`	`}`
`35`	`49`
`@@ -40,7 +54,7 @@ NodeStatus ReactiveFallback::tick()`
`40`	`54`
`41`	`55`	`case NodeStatus::SUCCESS:`
`42`	`56`	`{`
`43`		`- haltChildren(0);`
	`57`	`+ halt();`
`44`	`58`	`return NodeStatus::SUCCESS;`
`45`	`59`	`}`
`46`	`60`
`@@ -51,13 +65,12 @@ NodeStatus ReactiveFallback::tick()`
`51`	`65`	`} // end switch`
`52`	`66`	`} //end for`
`53`	`67`
`54`		`- if( failure_count == childrenCount() )`
	`68`	`+ if( failure_count != childrenCount() )`
`55`	`69`	`{`
`56`		`- haltChildren(0);`
`57`		`- return NodeStatus::FAILURE;`
	`70`	`+ throw LogicError("This is not supposed to happen");`
`58`	`71`	`}`
`59`		`-`
`60`		`- return NodeStatus::RUNNING;`
	`72`	`+ halt();`
	`73`	`+ return NodeStatus::FAILURE;`
`61`	`74`	`}`
`62`	`75`
`63`	`76`	`}`
Original file line number	Diff line number	Diff line change
`@@ -25,12 +25,12 @@ NodeStatus ReactiveSequence::tick()`
`25`	`25`	`{`
`26`	`26`	`is_asynch_child_.resize(childrenCount(), false);`
`27`	`27`	`size_t success_count = 0;`
`28`		`- size_t running_count = 0;`
`29`	`28`
`30`	`29`	`for (size_t index = 0; index < childrenCount(); index++)`
`31`	`30`	`{`
`32`	`31`	`TreeNode* current_child_node = children_nodes_[index];`
`33`		`- if( is_asynch_child_[index] && current_child_node->status() == NodeStatus::SUCCESS )`
	`32`	`+ if( is_asynch_child_[index] &&`
	`33`	`+ current_child_node->status() == NodeStatus::SUCCESS )`
`34`	`34`	`{`
`35`	`35`	`success_count++;`
`36`	`36`	`continue; // skip already executed asynch children`
`@@ -43,7 +43,6 @@ NodeStatus ReactiveSequence::tick()`
`43`	`43`	`case NodeStatus::RUNNING:`
`44`	`44`	`{`
`45`	`45`	`is_asynch_child_[index] = true;`
`46`		`- running_count++;`
`47`	`46`	`haltChildren(index+1);`
`48`	`47`	`return NodeStatus::RUNNING;`
`49`	`48`	`}`
`@@ -65,12 +64,12 @@ NodeStatus ReactiveSequence::tick()`
`65`	`64`	`} // end switch`
`66`	`65`	`} //end for`
`67`	`66`
`68`		`- if( success_count == childrenCount())`
	`67`	`+ if( success_count != childrenCount())`
`69`	`68`	`{`
`70`		`- halt();`
`71`		`- return NodeStatus::SUCCESS;`
	`69`	`+ throw LogicError("This is not supposed to happen");`
`72`	`70`	`}`
`73`		`- return NodeStatus::RUNNING;`
	`71`	`+ halt();`
	`72`	`+ return NodeStatus::SUCCESS;`
`74`	`73`	`}`
`75`	`74`
`76`	`75`
Original file line number	Diff line number	Diff line change
`@@ -415,7 +415,6 @@ TEST_F(ReactiveSequenceTest, ReactiveSequence)`
`415`	`415`	`ASSERT_EQ(NodeStatus::FAILURE, state);`
`416`	`416`	`ASSERT_GE( elapsed_ms, 100 );`
`417`	`417`	`ASSERT_LE( elapsed_ms, 120 );`
`418`		`-`
`419`	`418`	`}`
`420`	`419`
`421`	`420`