Allow BT factory to define clock source for TimerQueue/TimerNode (BehaviorTree#215)

fultoncjb · Cam Fulton · facontidavide · web-flow · commit 63d6fd42a1c4 · 2020-09-01T09:50:45.000+02:00
* Allow BT factory to define clock source for TimerQueue/TimerNode

* Fix unit tests

Co-authored-by: Cam Fulton &lt;cfulton@symbotic.com&gt;
Co-authored-by: Davide Faconti &lt;davide.faconti@gmail.com&gt;
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -161,7 +161,6 @@ list(APPEND BT_SOURCE
     src/decorators/repeat_node.cpp
     src/decorators/retry_node.cpp
     src/decorators/subtree_node.cpp
-    src/decorators/timeout_node.cpp
     src/decorators/delay_node.cpp
 
     src/controls/if_then_else_node.cpp
diff --git a/include/behaviortree_cpp_v3/decorators/timeout_node.h b/include/behaviortree_cpp_v3/decorators/timeout_node.h
@@ -10,7 +10,7 @@ namespace BT
 /**
  * @brief The TimeoutNode will halt() a running child if
  * the latter has been RUNNING for more than a give time.
- * The timeout is in millisecons and it is passed using the port "msec".
+ * The timeout is in milliseconds and it is passed using the port "msec".
  *
  * If timeout is reached it returns FAILURE.
  *
@@ -20,12 +20,30 @@ namespace BT
  *    <KeepYourBreath/>
  * </Timeout>
  */
+template <typename _Clock = std::chrono::steady_clock, typename _Duration = std::chrono::steady_clock::duration>
 class TimeoutNode : public DecoratorNode
 {
   public:
-    TimeoutNode(const std::string& name, unsigned milliseconds);
+    TimeoutNode(const std::string& name, unsigned milliseconds)
+    : DecoratorNode(name, {} ),
+      child_halted_(false),
+      timer_id_(0),
+      msec_(milliseconds),
+      read_parameter_from_ports_(false),
+      timeout_started_(false)
+    {
+        setRegistrationID("Timeout");
+    }
 
-    TimeoutNode(const std::string& name, const NodeConfiguration& config);
+    TimeoutNode(const std::string& name, const NodeConfiguration& config)
+    : DecoratorNode(name, config),
+      child_halted_(false),
+      timer_id_(0),
+      msec_(0),
+      read_parameter_from_ports_(true),
+      timeout_started_(false)
+    {
+    }
 
     ~TimeoutNode() override
     {
@@ -39,9 +57,59 @@ class TimeoutNode : public DecoratorNode
     }
 
   private:
-    TimerQueue timer_ ;
+    TimerQueue<_Clock, _Duration> timer_ ;
+
+    virtual BT::NodeStatus tick() override
+    {
+        if( read_parameter_from_ports_ )
+        {
+            if( !getInput("msec", msec_) )
+            {
+                throw RuntimeError("Missing parameter [msec] in TimeoutNode");
+            }
+        }
+
+        if ( !timeout_started_ )
+        {
+            timeout_started_ = true;
+            setStatus(NodeStatus::RUNNING);
+            child_halted_ = false;
+
+            if (msec_ > 0)
+            {
+                timer_id_ = timer_.add(std::chrono::milliseconds(msec_),
+                                       [this](bool aborted)
+                                       {
+                                           std::unique_lock<std::mutex> lk( timeout_mutex_ );
+                                           if (!aborted && child()->status() == NodeStatus::RUNNING)
+                                           {
+                                               child_halted_ = true;
+                                               haltChild();
+                                           }
+                                       });
+            }
+        }
 
-    virtual BT::NodeStatus tick() override;
+        std::unique_lock<std::mutex> lk( timeout_mutex_ );
+
+        if (child_halted_)
+        {
+            timeout_started_ = false;
+            return NodeStatus::FAILURE;
+        }
+        else
+        {
+            auto child_status = child()->executeTick();
+            if (child_status != NodeStatus::RUNNING)
+            {
+                timeout_started_ = false;
+                timeout_mutex_.unlock();
+                timer_.cancel(timer_id_);
+                timeout_mutex_.lock();
+            }
+            return child_status;
+        }
+    }
 
     std::atomic<bool> child_halted_;
     uint64_t timer_id_;
diff --git a/include/behaviortree_cpp_v3/decorators/timer_queue.h b/include/behaviortree_cpp_v3/decorators/timer_queue.h
@@ -62,6 +62,7 @@ class Semaphore
 //  - Handlers are ALWAYS executed in the Timer Queue worker thread.
 //  - Handlers execution order is NOT guaranteed
 //
+template <typename _Clock, typename _Duration>
 class TimerQueue
 {
   public:
@@ -85,7 +86,7 @@ class TimerQueue
     uint64_t add(std::chrono::milliseconds milliseconds, std::function<void(bool)> handler)
     {
         WorkItem item;
-        item.end = Clock::now() + milliseconds;
+        item.end = _Clock::now() + milliseconds;
         item.handler = std::move(handler);
 
         std::unique_lock<std::mutex> lk(m_mtx);
@@ -118,7 +119,7 @@ class TimerQueue
             {
                 WorkItem newItem;
                 // Zero time, so it stays at the top for immediate execution
-                newItem.end = Clock::time_point();
+                newItem.end = std::chrono::time_point<_Clock, _Duration>();
                 newItem.id = 0;   // Means it is a canceled item
                 // Move the handler from item to newitem.
                 // Also, we need to manually set the handler to nullptr, since
@@ -150,7 +151,7 @@ class TimerQueue
         {
             if (item.id)
             {
-                item.end = Clock::time_point();
+                item.end = std::chrono::time_point<_Clock, _Duration>();
                 item.id = 0;
             }
         }
@@ -162,7 +163,6 @@ class TimerQueue
     }
 
   private:
-    using Clock = std::chrono::steady_clock;
     TimerQueue(const TimerQueue&) = delete;
     TimerQueue& operator=(const TimerQueue&) = delete;
 
@@ -193,7 +193,7 @@ class TimerQueue
         assert(m_items.size() == 0);
     }
 
-    std::pair<bool, Clock::time_point> calcWaitTime()
+    std::pair<bool, std::chrono::time_point<_Clock, _Duration>> calcWaitTime()
     {
         std::lock_guard<std::mutex> lk(m_mtx);
         while (m_items.size())
@@ -212,13 +212,13 @@ class TimerQueue
 
         // No items found, so return no wait time (causes the thread to wait
         // indefinitely)
-        return std::make_pair(false, Clock::time_point());
+        return std::make_pair(false, std::chrono::time_point<_Clock, _Duration>());
     }
 
     void checkWork()
     {
         std::unique_lock<std::mutex> lk(m_mtx);
-        while (m_items.size() && m_items.top().end <= Clock::now())
+        while (m_items.size() && m_items.top().end <= _Clock::now())
         {
             WorkItem item(std::move(m_items.top()));
             m_items.pop();
@@ -237,7 +237,7 @@ class TimerQueue
 
     struct WorkItem
     {
-        Clock::time_point end;
+        std::chrono::time_point<_Clock, _Duration> end;
         uint64_t id;   // id==0 means it was cancelled
         std::function<void(bool)> handler;
         bool operator>(const WorkItem& other) const
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
@@ -36,7 +36,7 @@ BehaviorTreeFactory::BehaviorTreeFactory()
     registerNodeType<RetryNode>("RetryUntilSuccesful");
     registerNodeType<KeepRunningUntilFailureNode>("KeepRunningUntilFailure");
     registerNodeType<RepeatNode>("Repeat");
-    registerNodeType<TimeoutNode>("Timeout");
+    registerNodeType<TimeoutNode<>>("Timeout");
     registerNodeType<DelayNode>("Delay");
 
     registerNodeType<ForceSuccessNode>("ForceSuccess");
diff --git a/src/decorators/timeout_node.cpp b/src/decorators/timeout_node.cpp
diff --git a/tests/gtest_coroutines.cpp b/tests/gtest_coroutines.cpp
@@ -116,7 +116,7 @@ TEST(CoroTest, do_action_timeout)
     BT::assignDefaultRemapping<SimpleCoroAction>(node_config_);
 
     SimpleCoroAction node( milliseconds(300), false, "Action", node_config_);
-    BT::TimeoutNode timeout("TimeoutAction", 200);
+    BT::TimeoutNode<> timeout("TimeoutAction", 200);
 
     timeout.setChild(&node);
 
@@ -137,7 +137,7 @@ TEST(CoroTest, sequence_child)
 
     SimpleCoroAction actionA( milliseconds(200), false, "action_A", node_config_);
     SimpleCoroAction actionB( milliseconds(200), false, "action_B", node_config_);
-    BT::TimeoutNode timeout("timeout", 300);
+    BT::TimeoutNode<> timeout("timeout", 300);
     BT::SequenceNode sequence("sequence");
 
     timeout.setChild(&sequence);
diff --git a/tests/gtest_decorator.cpp b/tests/gtest_decorator.cpp
@@ -20,7 +20,7 @@ using std::chrono::milliseconds;
 
 struct DeadlineTest : testing::Test
 {
-    BT::TimeoutNode root;
+    BT::TimeoutNode<> root;
     BT::AsyncActionTest action;
 
     DeadlineTest() : root("deadline", 300)
@@ -66,7 +66,7 @@ struct RetryTest : testing::Test
 
 struct TimeoutAndRetry : testing::Test
 {
-    BT::TimeoutNode timeout_root;
+    BT::TimeoutNode<> timeout_root;
     BT::RetryNode retry;
     BT::SyncActionTest action;
 

Original file line number	Diff line number	Diff line change
`@@ -62,6 +62,7 @@ class Semaphore`
`62`	`62`	`// - Handlers are ALWAYS executed in the Timer Queue worker thread.`
`63`	`63`	`// - Handlers execution order is NOT guaranteed`
`64`	`64`	`//`
	`65`	`+template <typename _Clock, typename _Duration>`
`65`	`66`	`class TimerQueue`
`66`	`67`	`{`
`67`	`68`	`public:`
`@@ -85,7 +86,7 @@ class TimerQueue`
`85`	`86`	`uint64_t add(std::chrono::milliseconds milliseconds, std::function<void(bool)> handler)`
`86`	`87`	`{`
`87`	`88`	`WorkItem item;`
`88`		`- item.end = Clock::now() + milliseconds;`
	`89`	`+ item.end = _Clock::now() + milliseconds;`
`89`	`90`	`item.handler = std::move(handler);`
`90`	`91`
`91`	`92`	`std::unique_lock<std::mutex> lk(m_mtx);`
`@@ -118,7 +119,7 @@ class TimerQueue`
`118`	`119`	`{`
`119`	`120`	`WorkItem newItem;`
`120`	`121`	`// Zero time, so it stays at the top for immediate execution`
`121`		`- newItem.end = Clock::time_point();`
	`122`	`+ newItem.end = std::chrono::time_point<_Clock, _Duration>();`
`122`	`123`	`newItem.id = 0; // Means it is a canceled item`
`123`	`124`	`// Move the handler from item to newitem.`
`124`	`125`	`// Also, we need to manually set the handler to nullptr, since`
`@@ -150,7 +151,7 @@ class TimerQueue`
`150`	`151`	`{`
`151`	`152`	`if (item.id)`
`152`	`153`	`{`
`153`		`- item.end = Clock::time_point();`
	`154`	`+ item.end = std::chrono::time_point<_Clock, _Duration>();`
`154`	`155`	`item.id = 0;`
`155`	`156`	`}`
`156`	`157`	`}`
`@@ -162,7 +163,6 @@ class TimerQueue`
`162`	`163`	`}`
`163`	`164`
`164`	`165`	`private:`
`165`		`- using Clock = std::chrono::steady_clock;`
`166`	`166`	`TimerQueue(const TimerQueue&) = delete;`
`167`	`167`	`TimerQueue& operator=(const TimerQueue&) = delete;`
`168`	`168`
`@@ -193,7 +193,7 @@ class TimerQueue`
`193`	`193`	`assert(m_items.size() == 0);`
`194`	`194`	`}`
`195`	`195`
`196`		`- std::pair<bool, Clock::time_point> calcWaitTime()`
	`196`	`+ std::pair<bool, std::chrono::time_point<_Clock, _Duration>> calcWaitTime()`
`197`	`197`	`{`
`198`	`198`	`std::lock_guard<std::mutex> lk(m_mtx);`
`199`	`199`	`while (m_items.size())`
`@@ -212,13 +212,13 @@ class TimerQueue`
`212`	`212`
`213`	`213`	`// No items found, so return no wait time (causes the thread to wait`
`214`	`214`	`// indefinitely)`
`215`		`- return std::make_pair(false, Clock::time_point());`
	`215`	`+ return std::make_pair(false, std::chrono::time_point<_Clock, _Duration>());`
`216`	`216`	`}`
`217`	`217`
`218`	`218`	`void checkWork()`
`219`	`219`	`{`
`220`	`220`	`std::unique_lock<std::mutex> lk(m_mtx);`
`221`		`- while (m_items.size() && m_items.top().end <= Clock::now())`
	`221`	`+ while (m_items.size() && m_items.top().end <= _Clock::now())`
`222`	`222`	`{`
`223`	`223`	`WorkItem item(std::move(m_items.top()));`
`224`	`224`	`m_items.pop();`
`@@ -237,7 +237,7 @@ class TimerQueue`
`237`	`237`
`238`	`238`	`struct WorkItem`
`239`	`239`	`{`
`240`		`- Clock::time_point end;`
	`240`	`+ std::chrono::time_point<_Clock, _Duration> end;`
`241`	`241`	`uint64_t id; // id==0 means it was cancelled`
`242`	`242`	`std::function<void(bool)> handler;`
`243`	`243`	`bool operator>(const WorkItem& other) const`
Original file line number	Diff line number	Diff line change
`@@ -20,7 +20,7 @@ using std::chrono::milliseconds;`
`20`	`20`
`21`	`21`	`struct DeadlineTest : testing::Test`
`22`	`22`	`{`
`23`		`- BT::TimeoutNode root;`
	`23`	`+ BT::TimeoutNode<> root;`
`24`	`24`	`BT::AsyncActionTest action;`
`25`	`25`
`26`	`26`	`DeadlineTest() : root("deadline", 300)`
`@@ -66,7 +66,7 @@ struct RetryTest : testing::Test`
`66`	`66`
`67`	`67`	`struct TimeoutAndRetry : testing::Test`
`68`	`68`	`{`
`69`		`- BT::TimeoutNode timeout_root;`
	`69`	`+ BT::TimeoutNode<> timeout_root;`
`70`	`70`	`BT::RetryNode retry;`
`71`	`71`	`BT::SyncActionTest action;`
`72`	`72`