BehaviorTree · facontidavide · Sep 1, 2020 · Aug 4, 2020 · Aug 4, 2020 · Sep 1, 2020
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;