From fc53fbdd689526a078812f3fe9d49e93ada7120f Mon Sep 17 00:00:00 2001
From: Simon Rasmussen <zetlon@gmail.com>
Date: Thu, 16 Feb 2017 22:49:58 +0100
Subject: [PATCH] Fixed consumer thread getting stuck in dequeue on shutdown

---
 include/ur_modern_driver/pipeline.h | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h
index 13a9b1b..df56421 100644
--- a/include/ur_modern_driver/pipeline.h
+++ b/include/ur_modern_driver/pipeline.h
@@ -49,7 +49,7 @@ private:
 
             for (auto& p : products) {
                 if (!_queue.try_enqueue(std::move(p))) {
-                    LOG_WARN("Pipeline owerflowed!");
+                    LOG_ERROR("Pipeline owerflowed!");
                 }
             }
 
@@ -57,6 +57,7 @@ private:
         }
         _producer.teardown_producer();
         //todo cleanup
+        LOG_DEBUG("Pipline producer ended");
     }
 
     void run_consumer()
@@ -64,12 +65,18 @@ private:
         _consumer.setup_consumer();
         unique_ptr<T> product;
         while (_running) {
-            _queue.wait_dequeue(product);
+            // 16000us timeout was chosen because we should
+            // roughly recieve messages at 125hz which is every
+            // 8ms == 8000us and double it for some error margin
+            if (!_queue.wait_dequeue_timed(product, 16000)) {
+                continue;
+            }
             if (!_consumer.consume(std::move(product)))
                 break;
         }
         _consumer.teardown_consumer();
         //todo cleanup
+        LOG_DEBUG("Pipline consumer ended");
     }
 
 public:
@@ -96,6 +103,8 @@ public:
         if (!_running)
             return;
 
+        LOG_DEBUG("Stopping pipeline");
+
         _consumer.stop_consumer();
         _producer.stop_producer();