Event based trigger introduced

facontidavide · facontidavide · commit 60420495344b · 2022-05-21T18:25:53.000+02:00
Added a new mechanism to emit "state changed" events that can "wake up" a tree.
In short, it just provide an interruptible "sleep" function.
diff --git a/docs/index.md b/docs/index.md
@@ -23,7 +23,7 @@ to visualize, record, replay and analyze state transitions.
 ## Community
 
 If this documentation doesn't answer your questions or if you simply want to
-connect with the community of BT.CPP users, visit https://discourse.behaviortree.dev/
+connect with the community of BT.CPP users, visit [discourse.behaviortree.dev](https://discourse.behaviortree.dev/).
 
 ## What is a Behavior Tree?
 
diff --git a/docs/tutorial_04_sequence.md b/docs/tutorial_04_sequence.md
@@ -63,11 +63,11 @@ NodeStatus MoveBaseAction::tick()
     int count = 0;
 
     // Pretend that "computing" takes 250 milliseconds.
-    // It is up to you to check periodicall _halt_requested and interrupt
+    // It is up to you to check periodically _halt_requested and interrupt
     // this tick() if it is true.
     while (!_halt_requested && count++ < 25)
     {
-        SleepMS(10);
+        std::this_thread::sleep_for( std::chrono::milliseconds(10) );
     }
 
     std::cout << "[ MoveBase: FINISHED ]" << std::endl;
@@ -105,6 +105,7 @@ The following example should use a simple `SequenceNode`.
 int main()
 {
     using namespace DummyNodes;
+    using std::chrono::milliseconds;
 
     BehaviorTreeFactory factory;
     factory.registerSimpleCondition("BatteryOK", std::bind(CheckBattery));
@@ -118,11 +119,11 @@ int main()
     std::cout << "\n--- 1st executeTick() ---" << std::endl;
     status = tree.tickRoot();
 
-    SleepMS(150);
+    tree.sleep( milliseconds(150) );
     std::cout << "\n--- 2nd executeTick() ---" << std::endl;
     status = tree.tickRoot();
 
-    SleepMS(150);
+    tree.sleep( milliseconds(150) );
     std::cout << "\n--- 3rd executeTick() ---" << std::endl;
     status = tree.tickRoot();
 
@@ -191,5 +192,15 @@ Expected output:
     Robot says: "mission completed!"
 ```
 
+## Event Driven trees?
+
+!!! important 
+    We used the command `tree.sleep()` instead of `std::this_thread::sleep_for()` for a reason.
+    
+The method `Tree::sleep()` should be preferred, because it can be interrupted by a Node in the tree when
+"something changed".
+Tree::sleep() will be interrupted when an `AsyncActionNode::tick()` is completed or, more generally,
+when the method `TreeNode::emitStateChanged()` is invoked.
+
 
 
diff --git a/docs/tutorial_05_subtrees.md b/docs/tutorial_05_subtrees.md
@@ -110,9 +110,16 @@ int main()
         while( status == NodeStatus::RUNNING)
         {
             status = tree.tickRoot();
-            CrossDoor::SleepMS(1);   // optional sleep to avoid "busy loops"
+            // IMPORTANT: you must always add some sleep if you call tickRoot()
+            // in a loop, to avoid using 100% of your CPU (busy loop).
+            // The method Tree::sleep() is recommended, because it can be
+            // interrupted by an internal event inside the tree.
+            tree.sleep( milliseconds(10) );
+        }
+        if( LOOP )
+        {
+            std::this_thread::sleep_for( milliseconds(1000) );
         }
-        CrossDoor::SleepMS(2000);
     }
     return 0;
 }
diff --git a/docs/tutorial_06_subtree_ports.md b/docs/tutorial_06_subtree_ports.md
@@ -78,7 +78,9 @@ int main()
     while( status == NodeStatus::RUNNING)
     {
         status = tree.tickRoot();
-        SleepMS(1);   // optional sleep to avoid "busy loops"
+        // IMPORTANT: add sleep to avoid busy loops.
+        // You should use Tree::sleep(). Don't be afraid to run this at 1 KHz.
+        tree.sleep( std::chrono::milliseconds(1) );
     }
 
     // let's visualize some information about the current state of the blackboards.
diff --git a/docs/tutorial_09_coroutines.md b/docs/tutorial_09_coroutines.md
@@ -147,10 +147,10 @@ int main()
     auto tree = factory.createTreeFromText(xml_text);
 
     //---------------------------------------
-    // keep executin tick until it returns etiher SUCCESS or FAILURE
+    // keep executing tick until it returns either SUCCESS or FAILURE
     while( tree.tickRoot() == NodeStatus::RUNNING)
     {
-        std::this_thread::sleep_for( Milliseconds(10) );
+        tree.sleep( std::chrono::milliseconds(10) );
     }
     return 0;
 }
diff --git a/examples/t04_reactive_sequence.cpp b/examples/t04_reactive_sequence.cpp
@@ -57,9 +57,11 @@ void Assert(bool condition)
         throw RuntimeError("this is not what I expected");
 }
 
+
 int main()
 {
     using namespace DummyNodes;
+    using std::chrono::milliseconds;
 
     BehaviorTreeFactory factory;
     factory.registerSimpleCondition("BatteryOK", std::bind(CheckBattery));
@@ -85,12 +87,12 @@ int main()
         status = tree.tickRoot();
         Assert(status == NodeStatus::RUNNING);
 
-        SleepMS(150);
+        tree.sleep( milliseconds(150) );
         std::cout << "\n--- 2nd executeTick() ---" << std::endl;
         status = tree.tickRoot();
         Assert(status == NodeStatus::RUNNING);
 
-        SleepMS(150);
+        tree.sleep( milliseconds(150) );
         std::cout << "\n--- 3rd executeTick() ---" << std::endl;
         status = tree.tickRoot();
         Assert(status == NodeStatus::SUCCESS);
diff --git a/examples/t05_crossdoor.cpp b/examples/t05_crossdoor.cpp
@@ -85,16 +85,24 @@ int main(int argc, char** argv)
 
     const bool LOOP = ( argc == 2 && strcmp( argv[1], "loop") == 0);
 
+    using std::chrono::milliseconds;
     do
     {
         NodeStatus status = NodeStatus::RUNNING;
         // Keep on ticking until you get either a SUCCESS or FAILURE state
         while( status == NodeStatus::RUNNING)
         {
             status = tree.tickRoot();
-            CrossDoor::SleepMS(1);   // optional sleep to avoid "busy loops"
+            // IMPORTANT: you must always add some sleep if you call tickRoot()
+            // in a loop, to avoid using 100% of your CPU (busy loop).
+            // The method Tree::sleep() is recommended, because it can be
+            // interrupted by an internal event inside the tree.
+            tree.sleep( milliseconds(10) );
+        }
+        if( LOOP )
+        {
+            std::this_thread::sleep_for( milliseconds(1000) );
         }
-        CrossDoor::SleepMS(1000);
     }
     while(LOOP);
 
diff --git a/examples/t06_subtree_port_remapping.cpp b/examples/t06_subtree_port_remapping.cpp
@@ -68,7 +68,9 @@ int main()
     while( status == NodeStatus::RUNNING)
     {
         status = tree.tickRoot();
-        SleepMS(1);   // optional sleep to avoid "busy loops"
+        // IMPORTANT: add sleep to avoid busy loops.
+        // You should use Tree::sleep(). Don't be afraid to run this at 1 KHz.
+        tree.sleep( std::chrono::milliseconds(1) );
     }
 
     // let's visualize some information about the current state of the blackboards.
diff --git a/examples/t09_async_actions_coroutines.cpp b/examples/t09_async_actions_coroutines.cpp
@@ -115,10 +115,10 @@ int main()
     auto tree = factory.createTreeFromText(xml_text);
 
     //---------------------------------------
-    // keep executin tick until it returns etiher SUCCESS or FAILURE
+    // keep executing tick until it returns either SUCCESS or FAILURE
     while( tree.tickRoot() == NodeStatus::RUNNING)
     {
-        std::this_thread::sleep_for( std::chrono::milliseconds(10) );
+        tree.sleep( std::chrono::milliseconds(10) );
     }
     return 0;
 }
diff --git a/include/behaviortree_cpp_v3/action_node.h b/include/behaviortree_cpp_v3/action_node.h
@@ -113,6 +113,9 @@ class SimpleActionNode : public SyncActionNode
  * - remember, with few exceptions, a halted AsyncAction must return NodeStatus::IDLE.
  *
  * For a complete example, look at __AsyncActionTest__ in action_test_node.h in the folder test.
+ *
+ * NOTE: when the thread is completed, i.e. the tick() returns its status,
+ * a TreeNode::emitStateChanged() will be called.
  */
 class AsyncActionNode : public ActionNodeBase
 {
@@ -136,7 +139,7 @@ class AsyncActionNode : public ActionNodeBase
 
     std::exception_ptr exptr_;
     std::atomic_bool halt_requested_;
-    std::future<NodeStatus> thread_handle_;
+    std::future<void> thread_handle_;
     std::mutex m_;
 };
 
diff --git a/include/behaviortree_cpp_v3/bt_factory.h b/include/behaviortree_cpp_v3/bt_factory.h
@@ -143,13 +143,22 @@ class Tree
     Tree(Tree&& other)
     {
         (*this) = std::move(other);
+        for(auto& node: nodes)
+        {
+            node->setWakeUpInstance( &wake_up_ );
+        }
     }
 
     Tree& operator=(Tree&& other)
     {
         nodes = std::move(other.nodes);
         blackboard_stack = std::move(other.blackboard_stack);
         manifests = std::move(other.manifests);
+
+        for(auto& node: nodes)
+        {
+            node->setWakeUpInstance( &wake_up_ );
+        }
         return *this;
     }
 
@@ -178,7 +187,7 @@ class Tree
     }
 
     NodeStatus tickRoot()
-    {
+    {       
       if(!rootNode())
       {
         throw RuntimeError("Empty Tree");
@@ -190,10 +199,17 @@ class Tree
       return ret;
     }
 
+    /// Sleep for a certain amount of time.
+    /// This sleep could be interrupted by the method
+    /// TreeNode::emitStateChanged()
+    void sleep(std::chrono::system_clock::duration timeout);
+
     ~Tree();
 
     Blackboard::Ptr rootBlackboard();
 
+private:
+    WakeUpSignal wake_up_;
 };
 
 class Parser;
diff --git a/include/behaviortree_cpp_v3/decorators/timeout_node.h b/include/behaviortree_cpp_v3/decorators/timeout_node.h
@@ -85,6 +85,7 @@ class TimeoutNode : public DecoratorNode
                                            {
                                                child_halted_ = true;
                                                haltChild();
+                                               emitStateChanged();
                                            }
                                        });
             }
diff --git a/include/behaviortree_cpp_v3/tree_node.h b/include/behaviortree_cpp_v3/tree_node.h
@@ -21,6 +21,7 @@
 #include "behaviortree_cpp_v3/basic_types.h"
 #include "behaviortree_cpp_v3/blackboard.h"
 #include "behaviortree_cpp_v3/utils/strcat.hpp"
+#include "behaviortree_cpp_v3/utils/wakeup_signal.hpp"
 
 #ifdef _MSC_VER 
 #pragma warning(disable : 4127) 
@@ -174,6 +175,9 @@ class TreeNode
 
     static Optional<StringView> getRemappedKey(StringView port_name, StringView remapping_value);
 
+    // Notify the tree should be ticked again()
+    void emitStateChanged();
+
   protected:
     /// Method to be implemented by the user
     virtual BT::NodeStatus tick() = 0;
@@ -186,6 +190,8 @@ class TreeNode
     // Only BehaviorTreeFactory should call this
     void setRegistrationID(StringView ID);
 
+    void setWakeUpInstance(WakeUpSignal* instance);
+
     void modifyPortsRemapping(const PortsRemapping& new_remapping);
 
     void setStatus(NodeStatus new_status);
@@ -210,6 +216,8 @@ class TreeNode
     PreTickOverrideCallback pre_condition_callback_;
 
     PostTickOverrideCallback post_condition_callback_;
+
+    WakeUpSignal* wake_up_ = nullptr;
 };
 
 //-------------------------------------------------------
diff --git a/include/behaviortree_cpp_v3/utils/wakeup_signal.hpp b/include/behaviortree_cpp_v3/utils/wakeup_signal.hpp
@@ -0,0 +1,53 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  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.
+*/
+
+#ifndef BEHAVIORTREECORE_WAKEUP_SIGNAL_HPP
+#define BEHAVIORTREECORE_WAKEUP_SIGNAL_HPP
+
+#include <chrono>
+#include <mutex>
+#include <condition_variable>
+
+namespace BT
+{
+
+class WakeUpSignal
+{
+public:
+    /// Return true if the
+    bool waitFor(std::chrono::system_clock::duration tm)
+    {
+        std::unique_lock<std::mutex> lk(mutex_);
+        ready_ = false;
+        return cv_.wait_for(lk, tm, [this]{ return ready_; });
+    }
+
+    void emitSignal()
+    {
+       {
+           std::lock_guard<std::mutex> lk(mutex_);
+           ready_ = true;
+       }
+       cv_.notify_all();
+    }
+
+private:
+
+    std::mutex mutex_;
+    std::condition_variable cv_;
+    bool ready_ = false;
+};
+
+}
+
+#endif // BEHAVIORTREECORE_WAKEUP_SIGNAL_HPP
diff --git a/sample_nodes/crossdoor_nodes.cpp b/sample_nodes/crossdoor_nodes.cpp
@@ -7,6 +7,11 @@ BT_REGISTER_NODES(factory)
     CrossDoor::RegisterNodes(factory);
 }
 
+inline void SleepMS(int ms)
+{
+    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
+}
+
 // For simplicity, in this example the status of the door is not shared
 // using ports and blackboards
 static bool _door_open   = false;
diff --git a/sample_nodes/crossdoor_nodes.h b/sample_nodes/crossdoor_nodes.h
@@ -6,10 +6,6 @@ using namespace BT;
 
 namespace CrossDoor
 {
-inline void SleepMS(int ms)
-{
-    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
-}
 
 BT::NodeStatus IsDoorOpen();
 
diff --git a/sample_nodes/movebase_node.cpp b/sample_nodes/movebase_node.cpp
@@ -22,11 +22,11 @@ BT::NodeStatus MoveBaseAction::tick()
     int count = 0;
 
     // Pretend that "computing" takes 250 milliseconds.
-    // It is up to you to check periodicall _halt_requested and interrupt
+    // It is up to you to check periodically _halt_requested and interrupt
     // this tick() if it is true.
     while (!_halt_requested && count++ < 25)
     {
-        SleepMS(10);
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
 
     std::cout << "[ MoveBase: FINISHED ]" << std::endl;
diff --git a/sample_nodes/movebase_node.h b/sample_nodes/movebase_node.h
@@ -9,11 +9,6 @@ struct Pose2D
     double x, y, theta;
 };
 
-inline void SleepMS(int ms)
-{
-    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
-}
-
 namespace BT
 {
 // This template specialization is needed only if you want
diff --git a/src/action_node.cpp b/src/action_node.cpp
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
diff --git a/src/tree_node.cpp b/src/tree_node.cpp
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
diff --git a/tests/gtest_wakeup.cpp b/tests/gtest_wakeup.cpp

Original file line number	Diff line number	Diff line change
`@@ -110,9 +110,16 @@ int main()`
`110`	`110`	`while( status == NodeStatus::RUNNING)`
`111`	`111`	`{`
`112`	`112`	`status = tree.tickRoot();`
`113`		`- CrossDoor::SleepMS(1); // optional sleep to avoid "busy loops"`
	`113`	`+ // IMPORTANT: you must always add some sleep if you call tickRoot()`
	`114`	`+ // in a loop, to avoid using 100% of your CPU (busy loop).`
	`115`	`+ // The method Tree::sleep() is recommended, because it can be`
	`116`	`+ // interrupted by an internal event inside the tree.`
	`117`	`+ tree.sleep( milliseconds(10) );`
	`118`	`+ }`
	`119`	`+ if( LOOP )`
	`120`	`+ {`
	`121`	`+ std::this_thread::sleep_for( milliseconds(1000) );`
`114`	`122`	`}`
`115`		`- CrossDoor::SleepMS(2000);`
`116`	`123`	`}`
`117`	`124`	`return 0;`
`118`	`125`	`}`
Original file line number	Diff line number	Diff line change
`@@ -78,7 +78,9 @@ int main()`
`78`	`78`	`while( status == NodeStatus::RUNNING)`
`79`	`79`	`{`
`80`	`80`	`status = tree.tickRoot();`
`81`		`- SleepMS(1); // optional sleep to avoid "busy loops"`
	`81`	`+ // IMPORTANT: add sleep to avoid busy loops.`
	`82`	`+ // You should use Tree::sleep(). Don't be afraid to run this at 1 KHz.`
	`83`	`+ tree.sleep( std::chrono::milliseconds(1) );`
`82`	`84`	`}`
`83`	`85`
`84`	`86`	`// let's visualize some information about the current state of the blackboards.`
Original file line number	Diff line number	Diff line change
`@@ -147,10 +147,10 @@ int main()`
`147`	`147`	`auto tree = factory.createTreeFromText(xml_text);`
`148`	`148`
`149`	`149`	`//---------------------------------------`
`150`		`- // keep executin tick until it returns etiher SUCCESS or FAILURE`
	`150`	`+ // keep executing tick until it returns either SUCCESS or FAILURE`
`151`	`151`	`while( tree.tickRoot() == NodeStatus::RUNNING)`
`152`	`152`	`{`
`153`		`- std::this_thread::sleep_for( Milliseconds(10) );`
	`153`	`+ tree.sleep( std::chrono::milliseconds(10) );`
`154`	`154`	`}`
`155`	`155`	`return 0;`
`156`	`156`	`}`
Original file line number	Diff line number	Diff line change
`@@ -85,16 +85,24 @@ int main(int argc, char** argv)`
`85`	`85`
`86`	`86`	`const bool LOOP = ( argc == 2 && strcmp( argv[1], "loop") == 0);`
`87`	`87`
	`88`	`+ using std::chrono::milliseconds;`
`88`	`89`	`do`
`89`	`90`	`{`
`90`	`91`	`NodeStatus status = NodeStatus::RUNNING;`
`91`	`92`	`// Keep on ticking until you get either a SUCCESS or FAILURE state`
`92`	`93`	`while( status == NodeStatus::RUNNING)`
`93`	`94`	`{`
`94`	`95`	`status = tree.tickRoot();`
`95`		`- CrossDoor::SleepMS(1); // optional sleep to avoid "busy loops"`
	`96`	`+ // IMPORTANT: you must always add some sleep if you call tickRoot()`
	`97`	`+ // in a loop, to avoid using 100% of your CPU (busy loop).`
	`98`	`+ // The method Tree::sleep() is recommended, because it can be`
	`99`	`+ // interrupted by an internal event inside the tree.`
	`100`	`+ tree.sleep( milliseconds(10) );`
	`101`	`+ }`
	`102`	`+ if( LOOP )`
	`103`	`+ {`
	`104`	`+ std::this_thread::sleep_for( milliseconds(1000) );`
`96`	`105`	`}`
`97`		`- CrossDoor::SleepMS(1000);`
`98`	`106`	`}`
`99`	`107`	`while(LOOP);`
`100`	`108`
Original file line number	Diff line number	Diff line change
`@@ -68,7 +68,9 @@ int main()`
`68`	`68`	`while( status == NodeStatus::RUNNING)`
`69`	`69`	`{`
`70`	`70`	`status = tree.tickRoot();`
`71`		`- SleepMS(1); // optional sleep to avoid "busy loops"`
	`71`	`+ // IMPORTANT: add sleep to avoid busy loops.`
	`72`	`+ // You should use Tree::sleep(). Don't be afraid to run this at 1 KHz.`
	`73`	`+ tree.sleep( std::chrono::milliseconds(1) );`
`72`	`74`	`}`
`73`	`75`
`74`	`76`	`// let's visualize some information about the current state of the blackboards.`
Original file line number	Diff line number	Diff line change
`@@ -115,10 +115,10 @@ int main()`
`115`	`115`	`auto tree = factory.createTreeFromText(xml_text);`
`116`	`116`
`117`	`117`	`//---------------------------------------`
`118`		`- // keep executin tick until it returns etiher SUCCESS or FAILURE`
	`118`	`+ // keep executing tick until it returns either SUCCESS or FAILURE`
`119`	`119`	`while( tree.tickRoot() == NodeStatus::RUNNING)`
`120`	`120`	`{`
`121`		`- std::this_thread::sleep_for( std::chrono::milliseconds(10) );`
	`121`	`+ tree.sleep( std::chrono::milliseconds(10) );`
`122`	`122`	`}`
`123`	`123`	`return 0;`
`124`	`124`	`}`