diff --git a/include/villas/queue.hpp b/include/villas/queue.hpp
index 7892a41e5..25c3a8a9b 100644
--- a/include/villas/queue.hpp
+++ b/include/villas/queue.hpp
@@ -24,7 +24,6 @@
 #pragma once
 
 #include <mutex>
-#include <condition_variable>
 #include <queue>
 
 namespace villas {
@@ -34,15 +33,20 @@ class Queue {
 
 protected:
 	std::queue<T> queue;
+	std::mutex mtx;
 
 public:
 	void push(T p)
 	{
+		std::unique_lock<std::mutex> guard(mtx);
+
 		queue.push(p);
 	}
 
 	T pop()
 	{
+		std::unique_lock<std::mutex> guard(mtx);
+
 		T res = queue.front();
 		queue.pop();
 
@@ -51,6 +55,8 @@ public:
 
 	bool empty()
 	{
+		std::unique_lock<std::mutex> guard(mtx);
+
 		return queue.empty();
 	}
 
diff --git a/include/villas/queue_signalled.hpp b/include/villas/queue_signalled.hpp
index 98a9e1f2a..91fede327 100644
--- a/include/villas/queue_signalled.hpp
+++ b/include/villas/queue_signalled.hpp
@@ -34,14 +34,11 @@ template<typename T>
 class QueueSignalled : public Queue<T> {
 
 private:
-	std::mutex mutex;
 	std::condition_variable cv;
 
 public:
 	void push(const T& data)
 	{
-		std::lock_guard<std::mutex> l(mutex);
-
 		Queue<T>::push(data);
 
 		cv.notify_one();
@@ -49,19 +46,15 @@ public:
 
 	T pop()
 	{
-		std::unique_lock<std::mutex> l(mutex);
+		std::unique_lock<std::mutex> l(Queue<T>::mtx);
 
-		while (Queue<T>::empty())
+		while (Queue<T>::queue.empty())
 			cv.wait(l);
 
-		return Queue<T>::pop();
-	}
+		T res = Queue<T>::queue.front();
+		Queue<T>::queue.pop();
 
-	bool empty()
-	{
-		std::unique_lock<std::mutex> l(mutex);
-
-		return Queue<T>::empty();
+		return res;
 	}
 };