fix issue BehaviorTree#57 (thread safety of Timeout)

Davide Faconti · Davide Faconti · commit a0f733ccbcc0 · 2019-02-25T16:09:34.000+01:00
diff --git a/include/behaviortree_cpp/decorators/timeout_node.h b/include/behaviortree_cpp/decorators/timeout_node.h
@@ -27,18 +27,19 @@ class TimeoutNode : public DecoratorNode
 
     TimeoutNode(const std::string& name, const NodeConfiguration& config);
 
+    ~TimeoutNode() override
+    {
+        timer_.cancelAll();
+    }
+
     static PortsList providedPorts()
     {
         return { InputPort<unsigned>("msec", "After a certain amount of time, "
                                              "halt() the child if it is still running.") };
     }
 
   private:
-    static TimerQueue& timer()
-    {
-        static TimerQueue timer_queue;
-        return timer_queue;
-    }
+    TimerQueue timer_ ;
 
     virtual BT::NodeStatus tick() override;
 
@@ -48,6 +49,7 @@ class TimeoutNode : public DecoratorNode
     unsigned msec_;
     bool read_parameter_from_ports_;
     bool timeout_started_;
+    std::mutex timeout_mutex_;
 };
 }
 
diff --git a/src/decorators/timeout_node.cpp b/src/decorators/timeout_node.cpp
@@ -54,9 +54,10 @@ NodeStatus TimeoutNode::tick()
 
         if (msec_ > 0)
         {
-            timer_id_ = timer().add(std::chrono::milliseconds(msec_),
+            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;
@@ -67,6 +68,8 @@ NodeStatus TimeoutNode::tick()
         }
     }
 
+    std::unique_lock<std::mutex> lk( timeout_mutex_ );
+
     if (child_halted_)
     {
         timeout_started_ = false;
@@ -77,8 +80,10 @@ NodeStatus TimeoutNode::tick()
         auto child_status = child()->executeTick();
         if (child_status != NodeStatus::RUNNING)
         {
-            timer().cancel(timer_id_);
             timeout_started_ = false;
+            timeout_mutex_.unlock();
+            timer_.cancel(timer_id_);
+            timeout_mutex_.lock();
         }
         return child_status;
     }

Original file line number	Diff line number	Diff line change
`@@ -54,9 +54,10 @@ NodeStatus TimeoutNode::tick()`
`54`	`54`
`55`	`55`	`if (msec_ > 0)`
`56`	`56`	`{`
`57`		`- timer_id_ = timer().add(std::chrono::milliseconds(msec_),`
	`57`	`+ timer_id_ = timer_.add(std::chrono::milliseconds(msec_),`
`58`	`58`	`[this](bool aborted)`
`59`	`59`	`{`
	`60`	`+ std::unique_lock<std::mutex> lk( timeout_mutex_ );`
`60`	`61`	`if (!aborted && child()->status() == NodeStatus::RUNNING)`
`61`	`62`	`{`
`62`	`63`	`child_halted_ = true;`
`@@ -67,6 +68,8 @@ NodeStatus TimeoutNode::tick()`
`67`	`68`	`}`
`68`	`69`	`}`
`69`	`70`
	`71`	`+ std::unique_lock<std::mutex> lk( timeout_mutex_ );`
	`72`	`+`
`70`	`73`	`if (child_halted_)`
`71`	`74`	`{`
`72`	`75`	`timeout_started_ = false;`
`@@ -77,8 +80,10 @@ NodeStatus TimeoutNode::tick()`
`77`	`80`	`auto child_status = child()->executeTick();`
`78`	`81`	`if (child_status != NodeStatus::RUNNING)`
`79`	`82`	`{`
`80`		`- timer().cancel(timer_id_);`
`81`	`83`	`timeout_started_ = false;`
	`84`	`+ timeout_mutex_.unlock();`
	`85`	`+ timer_.cancel(timer_id_);`
	`86`	`+ timeout_mutex_.lock();`
`82`	`87`	`}`
`83`	`88`	`return child_status;`
`84`	`89`	`}`