addrss problem with ReactiveSequence

Davide Faconti · Davide Faconti · commit aed23b7a2892 · 2019-04-03T10:21:40.000+02:00
diff --git a/include/behaviortree_cpp/controls/reactive_sequence.h b/include/behaviortree_cpp/controls/reactive_sequence.h
@@ -38,9 +38,12 @@ class ReactiveSequence : public ControlNode
     ReactiveSequence(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_sequence.cpp b/src/controls/reactive_sequence.cpp
@@ -15,28 +15,42 @@
 namespace BT
 {
 
+void ReactiveSequence::halt()
+{
+    std::fill(is_asynch_child_.begin(), is_asynch_child_.end(), false );
+    ControlNode::halt();
+}
+
 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 )
+        {
+            success_count++;
+            continue; // skip already executed asynch children
+        }
+
         const NodeStatus child_status = current_child_node->executeTick();
 
         switch (child_status)
         {
             case NodeStatus::RUNNING:
             {
+                is_asynch_child_[index] = true;
                 running_count++;
                 haltChildren(index+1);
                 return NodeStatus::RUNNING;
             }
 
             case NodeStatus::FAILURE:
             {
-                haltChildren(0);
+                halt();
                 return NodeStatus::FAILURE;
             }
             case NodeStatus::SUCCESS:
@@ -53,7 +67,7 @@ NodeStatus ReactiveSequence::tick()
 
     if( success_count == childrenCount())
     {
-        haltChildren(0);
+        halt();
         return NodeStatus::SUCCESS;
     }
     return NodeStatus::RUNNING;
diff --git a/tests/gtest_sequence.cpp b/tests/gtest_sequence.cpp
@@ -67,6 +67,34 @@ struct ComplexSequenceTest : testing::Test
     }
 };
 
+struct ReactiveSequenceTest : testing::Test
+{
+    BT::ReactiveSequence root;
+    BT::AsyncActionTest action_1;
+    BT::AsyncActionTest action_2;
+    BT::ConditionTestNode condition_1;
+    BT::ConditionTestNode condition_2;
+
+    ReactiveSequenceTest()
+      : root("root")
+      , action_1("action_1", milliseconds(100))
+      , action_2("action_1", milliseconds(100))
+      , condition_1("condition_1")
+      , condition_2("condition_2")
+    {
+        root.addChild(&condition_1);
+        root.addChild(&action_1);
+        root.addChild(&condition_2);
+        root.addChild(&action_2);
+
+    }
+    ~ReactiveSequenceTest()
+    {
+        haltAllActions(&root);
+    }
+};
+
+
 struct SequenceTripleActionTest : testing::Test
 {
     BT::SequenceNode root;
@@ -348,6 +376,50 @@ TEST_F(ComplexSequenceTest, ComplexSequenceConditions2ToFalse)
     ASSERT_EQ(NodeStatus::IDLE, action_1.status());
 }
 
+TEST_F(ReactiveSequenceTest, ReactiveSequence)
+{
+    // actions take 100 ms, therefore a succesfull sequence should take 200 ms
+    BT::NodeStatus state = root.executeTick();
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    auto begin_time = std::chrono::steady_clock::now();
+    while( state == NodeStatus::RUNNING)
+    {
+        state = root.executeTick();
+        std::this_thread::sleep_for( milliseconds(5) );
+    }
+    auto end_time = std::chrono::steady_clock::now();
+    int elapsed_ms = std::chrono::duration_cast<milliseconds>(end_time - begin_time).count();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+    ASSERT_GE( elapsed_ms, 200 );
+    ASSERT_LE( elapsed_ms, 220 );
+
+    //---------------------
+    root.halt();
+    // action_1 is executed, but action_2 isn't. Total expected time 100 ms
+    condition_2.setBoolean( false );
+
+    state = root.executeTick();
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    begin_time = std::chrono::steady_clock::now();
+    while( state == NodeStatus::RUNNING)
+    {
+        state = root.executeTick();
+        std::this_thread::sleep_for( milliseconds(5) );
+    }
+    end_time = std::chrono::steady_clock::now();
+    elapsed_ms = std::chrono::duration_cast<milliseconds>(end_time - begin_time).count();
+
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+    ASSERT_GE( elapsed_ms, 100 );
+    ASSERT_LE( elapsed_ms, 120 );
+
+}
+
+
+
 TEST_F(SimpleSequenceWithMemoryTest, ConditionTrue)
 {
     BT::NodeStatus state = root.executeTick();

Original file line number	Diff line number	Diff line change
`@@ -15,28 +15,42 @@`
`15`	`15`	`namespace BT`
`16`	`16`	`{`
`17`	`17`
	`18`	`+void ReactiveSequence::halt()`
	`19`	`+{`
	`20`	`+ std::fill(is_asynch_child_.begin(), is_asynch_child_.end(), false );`
	`21`	`+ ControlNode::halt();`
	`22`	`+}`
	`23`	`+`
`18`	`24`	`NodeStatus ReactiveSequence::tick()`
`19`	`25`	`{`
	`26`	`+ is_asynch_child_.resize(childrenCount(), false);`
`20`	`27`	`size_t success_count = 0;`
`21`	`28`	`size_t running_count = 0;`
`22`	`29`
`23`	`30`	`for (size_t index = 0; index < childrenCount(); index++)`
`24`	`31`	`{`
`25`	`32`	`TreeNode* current_child_node = children_nodes_[index];`
	`33`	`+ if( is_asynch_child_[index] && current_child_node->status() == NodeStatus::SUCCESS )`
	`34`	`+ {`
	`35`	`+ success_count++;`
	`36`	`+ continue; // skip already executed asynch children`
	`37`	`+ }`
	`38`	`+`
`26`	`39`	`const NodeStatus child_status = current_child_node->executeTick();`
`27`	`40`
`28`	`41`	`switch (child_status)`
`29`	`42`	`{`
`30`	`43`	`case NodeStatus::RUNNING:`
`31`	`44`	`{`
	`45`	`+ is_asynch_child_[index] = true;`
`32`	`46`	`running_count++;`
`33`	`47`	`haltChildren(index+1);`
`34`	`48`	`return NodeStatus::RUNNING;`
`35`	`49`	`}`
`36`	`50`
`37`	`51`	`case NodeStatus::FAILURE:`
`38`	`52`	`{`
`39`		`- haltChildren(0);`
	`53`	`+ halt();`
`40`	`54`	`return NodeStatus::FAILURE;`
`41`	`55`	`}`
`42`	`56`	`case NodeStatus::SUCCESS:`
`@@ -53,7 +67,7 @@ NodeStatus ReactiveSequence::tick()`
`53`	`67`
`54`	`68`	`if( success_count == childrenCount())`
`55`	`69`	`{`
`56`		`- haltChildren(0);`
	`70`	`+ halt();`
`57`	`71`	`return NodeStatus::SUCCESS;`
`58`	`72`	`}`
`59`	`73`	`return NodeStatus::RUNNING;`