c_sync: using std::thread;

Signed-off-by: Benn Snyder <[email protected]>

Author: piedar

wrappers/c_sync/CMakeLists.txt

@@ -2,12 +2,15 @@
 # C Synchronous Interface
 ######################################################################################
 
-set(THREADS_USE_PTHREADS_WIN32 true)
+cmake_minimum_required(VERSION 3.5)
+
 find_package(Threads REQUIRED)
-include_directories(${THREADS_PTHREADS_INCLUDE_DIR})
+list(APPEND CompileFeatures "cxx_alias_templates" "cxx_nullptr")
 
-add_library (freenect_sync SHARED libfreenect_sync.c)
-add_library (freenect_sync_static STATIC libfreenect_sync.c)
+add_library(freenect_sync        SHARED libfreenect_sync.cpp)
+add_library(freenect_sync_static STATIC libfreenect_sync.cpp)
+target_compile_features(freenect_sync        PRIVATE ${CompileFeatures})
+target_compile_features(freenect_sync_static PRIVATE ${CompileFeatures})
 set_target_properties (freenect_sync_static PROPERTIES OUTPUT_NAME freenect_sync)
 
 set_target_properties (freenect_sync PROPERTIES

wrappers/c_sync/libfreenect_sync.c renamed to wrappers/c_sync/libfreenect_sync.cpp

@@ -23,20 +23,37 @@
  * Binary distributions must follow the binary distribution requirements of
  * either License.
  */
-#include <stdio.h>
-#include <pthread.h>
-#include <string.h>
-#include <stdlib.h>
-#include <assert.h>
+#include <cassert>
+#include <condition_variable>
+#include <iostream>
+#include <thread>
+#include <mutex>
 #include "libfreenect_registration.h"
 #include "libfreenect_sync.h"
 
+using std::cerr;
+using std::endl;
+using Mutex = std::mutex;
+using UniqueLock = std::unique_lock<Mutex>;
+using LockGuard = std::lock_guard<Mutex>;
+using ConditionVariable = std::condition_variable;
+using Thread = std::thread;
+
+namespace
+{
+	void FN_WARN_INVALID_INDEX(const int index)
+	{
+		cerr << "Error: Invalid index [" << index << "]" << endl;
+	}
+}
+
+
 typedef struct buffer_ring {
-	pthread_mutex_t lock;
-	pthread_cond_t cb_cond;
+	Mutex mutex;
+	ConditionVariable cb_cond;
 	void *bufs[3];
 	uint32_t timestamp;
-	int valid; // True if middle buffer is valid
+	bool valid; // True if middle buffer is valid
 	int fmt;
 	int res;
 } buffer_ring_t;
@@ -47,102 +64,113 @@ typedef struct sync_kinect {
 	buffer_ring_t depth;
 } sync_kinect_t;
 
+struct AutomaticShutdownThread : public Thread
+{
+	using Thread::thread;
+
+	virtual ~AutomaticShutdownThread()
+	{
+		freenect_sync_stop();
+	}
+
+	AutomaticShutdownThread& operator=(Thread&& other)
+	{
+		Thread::operator=(std::move(other));
+		return *this;
+	}
+};
+
 typedef int (*set_buffer_t)(freenect_device *dev, void *buf);
 
 #define MAX_KINECTS 64
 static sync_kinect_t *kinects[MAX_KINECTS] = { 0 };
 static freenect_context *ctx;
-static int thread_running = 0;
-static pthread_t thread;
-static pthread_mutex_t runloop_lock = PTHREAD_MUTEX_INITIALIZER;
+static bool thread_running = false;
+static AutomaticShutdownThread thread;
+static Mutex runloop_mutex;
 static int pending_runloop_tasks = 0;
-static pthread_mutex_t pending_runloop_tasks_lock = PTHREAD_MUTEX_INITIALIZER;
-static pthread_cond_t pending_runloop_tasks_cond = PTHREAD_COND_INITIALIZER;
-
-/* Locking Convention
-   Rules:
-       - if you need more than one lock on a line, get them from left to right
-       - do not mix locks on different lines
-       - if you need to change the lock rules, make sure you check everything and update this
-   Lock Families:
-       - pending_runloop_tasks_lock
-       - runloop_lock, buffer_ring_t.lock (NOTE: You may only have one)
+static Mutex pending_runloop_tasks_mutex;
+static ConditionVariable pending_runloop_tasks_cond;
+
+/* Lock Families:
+       - pending_runloop_tasks_mutex
+       - runloop_mutex, buffer_ring_t.mutex (NOTE: You may only have one)
 */
 
 static int alloc_buffer_ring_video(freenect_resolution res, freenect_video_format fmt, buffer_ring_t *buf)
 {
-	int sz, i;
+	int size;
 	switch (fmt) {
 		case FREENECT_VIDEO_RGB:
 		case FREENECT_VIDEO_BAYER:
 		case FREENECT_VIDEO_IR_8BIT:
 		case FREENECT_VIDEO_IR_10BIT:
 		case FREENECT_VIDEO_IR_10BIT_PACKED:
-			sz = freenect_find_video_mode(res, fmt).bytes;
+			size = freenect_find_video_mode(res, fmt).bytes;
 			break;
 		default:
-			printf("Invalid video format %d\n", fmt);
+			cerr << "Invalid video format " << fmt << endl;
 			return -1;
 	}
-	for (i = 0; i < 3; ++i)
-		buf->bufs[i] = malloc(sz);
+	for (short i = 0; i < 3; ++i) {
+		buf->bufs[i] = malloc(size);
+	}
 	buf->timestamp = 0;
-	buf->valid = 0;
+	buf->valid = false;
 	buf->fmt = fmt;
 	buf->res = res;
 	return 0;
 }
 
 static int alloc_buffer_ring_depth(freenect_resolution res, freenect_depth_format fmt, buffer_ring_t *buf)
 {
-	int sz, i;
+	int size;
 	switch (fmt) {
 		case FREENECT_DEPTH_11BIT:
 		case FREENECT_DEPTH_10BIT:
 		case FREENECT_DEPTH_11BIT_PACKED:
 		case FREENECT_DEPTH_10BIT_PACKED:
 		case FREENECT_DEPTH_REGISTERED:
 		case FREENECT_DEPTH_MM:
-			sz = freenect_find_depth_mode(res, fmt).bytes;
+			size = freenect_find_depth_mode(res, fmt).bytes;
 			break;
 		default:
-			printf("Invalid depth format %d\n", fmt);
+			cerr << "Invalid depth format " << fmt << endl;
 			return -1;
 	}
-	for (i = 0; i < 3; ++i)
-		buf->bufs[i] = malloc(sz);
+	for (short i = 0; i < 3; ++i) {
+		buf->bufs[i] = malloc(size);
+	}
 	buf->timestamp = 0;
-	buf->valid = 0;
+	buf->valid = false;
 	buf->fmt = fmt;
 	buf->res = res;
 	return 0;
 }
 
 static void free_buffer_ring(buffer_ring_t *buf)
 {
-	int i;
-	for (i = 0; i < 3; ++i) {
+	for (short i = 0; i < 3; ++i) {
 		free(buf->bufs[i]);
-		buf->bufs[i] = NULL;
+		buf->bufs[i] = nullptr;
 	}
 	buf->timestamp = 0;
-	buf->valid = 0;
+	buf->valid = false;
 	buf->fmt = -1;
 	buf->res = -1;
 }
 
 static void producer_cb_inner(freenect_device *dev, void *data, uint32_t timestamp, buffer_ring_t *buf, set_buffer_t set_buffer)
 {
-	pthread_mutex_lock(&buf->lock);
+	UniqueLock buffer_lock(buf->mutex);
 	assert(data == buf->bufs[2]);
 	void *temp_buf = buf->bufs[1];
 	buf->bufs[1] = buf->bufs[2];
 	buf->bufs[2] = temp_buf;
 	set_buffer(dev, temp_buf);
 	buf->timestamp = timestamp;
-	buf->valid = 1;
-	pthread_cond_signal(&buf->cb_cond);
-	pthread_mutex_unlock(&buf->lock);
+	buf->valid = true;
+	buf->cb_cond.notify_one();
 }
 
 static void video_producer_cb(freenect_device *dev, void *data, uint32_t timestamp)
@@ -155,71 +183,69 @@ static void depth_producer_cb(freenect_device *dev, void *data, uint32_t timesta
 	producer_cb_inner(dev, data, timestamp, &((sync_kinect_t *)freenect_get_user(dev))->depth, freenect_set_depth_buffer);
 }
 
-/* You should only use these functions to manipulate the pending_runloop_tasks_lock*/
+/* You should only use these functions to manipulate the pending_runloop_tasks_mutex*/
 static void pending_runloop_tasks_inc(void)
 {
-	pthread_mutex_lock(&pending_runloop_tasks_lock);
+	LockGuard lock(pending_runloop_tasks_mutex);
 	assert(pending_runloop_tasks >= 0);
 	++pending_runloop_tasks;
-	pthread_mutex_unlock(&pending_runloop_tasks_lock);
 }
 
 static void pending_runloop_tasks_dec(void)
 {
-	pthread_mutex_lock(&pending_runloop_tasks_lock);
+	LockGuard lock(pending_runloop_tasks_mutex);
 	--pending_runloop_tasks;
 	assert(pending_runloop_tasks >= 0);
-	if (!pending_runloop_tasks)
-		pthread_cond_signal(&pending_runloop_tasks_cond);
-	pthread_mutex_unlock(&pending_runloop_tasks_lock);
+	if (!pending_runloop_tasks) {
+		pending_runloop_tasks_cond.notify_one();
+	}
 }
 
 static void pending_runloop_tasks_wait_zero(void)
 {
-	pthread_mutex_lock(&pending_runloop_tasks_lock);
-	while (pending_runloop_tasks)
-		pthread_cond_wait(&pending_runloop_tasks_cond, &pending_runloop_tasks_lock);
-	pthread_mutex_unlock(&pending_runloop_tasks_lock);
+	UniqueLock lock(pending_runloop_tasks_mutex);
+	while (pending_runloop_tasks) {
+		pending_runloop_tasks_cond.wait(lock);
+	}
 }
 
-static void *init(void *unused)
+static void runloop()
 {
 	pending_runloop_tasks_wait_zero();
-	pthread_mutex_lock(&runloop_lock);
+	UniqueLock runloop_lock(runloop_mutex);
+
 	while (thread_running && freenect_process_events(ctx) >= 0) {
-		pthread_mutex_unlock(&runloop_lock);
 		// NOTE: This lets you run tasks while process_events isn't running
+		runloop_lock.unlock();
 		pending_runloop_tasks_wait_zero();
-		pthread_mutex_lock(&runloop_lock);
+		runloop_lock.lock();
 	}
 	// Go through each device, call stop video, close device
-	int i;
-	for (i = 0; i < MAX_KINECTS; ++i) {
+	for (short i = 0; i < MAX_KINECTS; ++i) {
 		if (kinects[i]) {
 			freenect_stop_video(kinects[i]->dev);
 			freenect_stop_depth(kinects[i]->dev);
-			freenect_set_user(kinects[i]->dev, NULL);
+			freenect_set_user(kinects[i]->dev, nullptr);
 			freenect_close_device(kinects[i]->dev);
 			free_buffer_ring(&kinects[i]->video);
 			free_buffer_ring(&kinects[i]->depth);
 			free(kinects[i]);
-			kinects[i] = NULL;
+			kinects[i] = nullptr;
 		}
 	}
 	freenect_shutdown(ctx);
-	pthread_mutex_unlock(&runloop_lock);
-	return NULL;
 }
 
 static void init_thread(void)
 {
-	thread_running = 1;
+	assert(!thread_running);
+	thread_running = true;
 	freenect_init(&ctx, 0);
 	// We claim both the motor and the camera, because we can't know in advance
 	// which devices the caller will want, and the c_sync interface doesn't
 	// support audio, so there's no reason to claim the device needlessly.
 	freenect_select_subdevices(ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
-	pthread_create(&thread, NULL, init, NULL);
+	thread = Thread(runloop);
 }
 
 static int change_video_format(sync_kinect_t *kinect, freenect_resolution res, freenect_video_format fmt)
@@ -248,124 +274,116 @@ static int change_depth_format(sync_kinect_t *kinect, freenect_resolution res, f
 
 static sync_kinect_t *alloc_kinect(int index)
 {
-	sync_kinect_t *kinect = (sync_kinect_t*)malloc(sizeof(sync_kinect_t));
+	sync_kinect_t* kinect = new sync_kinect_t();
 	if (freenect_open_device(ctx, &kinect->dev, index)) {
-		free(kinect);
-		return NULL;
+		delete kinect;
+		return nullptr;
 	}
-	int i;
-	for (i = 0; i < 3; ++i) {
-		kinect->video.bufs[i] = NULL;
-		kinect->depth.bufs[i] = NULL;
+	for (short i = 0; i < 3; ++i) {
+		kinect->video.bufs[i] = nullptr;
+		kinect->depth.bufs[i] = nullptr;
 	}
 	kinect->video.fmt = -1;
 	kinect->video.res = -1;
 	kinect->depth.fmt = -1;
 	kinect->depth.res = -1;
 	freenect_set_video_callback(kinect->dev, video_producer_cb);
 	freenect_set_depth_callback(kinect->dev, depth_producer_cb);
-	pthread_mutex_init(&kinect->video.lock, NULL);
-	pthread_mutex_init(&kinect->depth.lock, NULL);
-	pthread_cond_init(&kinect->video.cb_cond, NULL);
-	pthread_cond_init(&kinect->depth.cb_cond, NULL);
 	return kinect;
 }
 
 static int setup_kinect(int index, int res, int fmt, int is_depth)
 {
 	pending_runloop_tasks_inc();
-	pthread_mutex_lock(&runloop_lock);
-	int thread_running_prev = thread_running;
+	UniqueLock runloop_lock(runloop_mutex);
+
+	bool thread_running_prev = thread_running;
 	if (!thread_running)
 		init_thread();
 	if (!kinects[index]) {
 		kinects[index] = alloc_kinect(index);
 	}
 	if (!kinects[index]) {
-		printf("Error: Invalid index [%d]\n", index);
+		FN_WARN_INVALID_INDEX(index);
 		// If we started the thread, we need to bring it back
 		if (!thread_running_prev) {
-			thread_running = 0;
-			pthread_mutex_unlock(&runloop_lock);
+			thread_running = false;
+			runloop_lock.unlock();
 			pending_runloop_tasks_dec();
-			pthread_join(thread, NULL);
+			thread.join();
 		} else {
-			pthread_mutex_unlock(&runloop_lock);
+			runloop_lock.unlock();
 			pending_runloop_tasks_dec();
 		}
 		return -1;
 	}
 	freenect_set_user(kinects[index]->dev, kinects[index]);
-	buffer_ring_t *buf;
-	if (is_depth)
-		buf = &kinects[index]->depth;
-	else
-		buf = &kinects[index]->video;
-	pthread_mutex_lock(&buf->lock);
+	buffer_ring_t *buf = is_depth ? &kinects[index]->depth : &kinects[index]->video;
+
+	UniqueLock buffer_lock(buf->mutex);
 	if ((buf->fmt != fmt) || (buf->res != res))
 	{
 		if (is_depth)
 			change_depth_format(kinects[index], (freenect_resolution)res, (freenect_depth_format)fmt);
 		else
 			change_video_format(kinects[index], (freenect_resolution)res, (freenect_video_format)fmt);
 	}
-	pthread_mutex_unlock(&buf->lock);
-	pthread_mutex_unlock(&runloop_lock);
+	buffer_lock.unlock();
 	pending_runloop_tasks_dec();
 	return 0;
 }
 
 static int sync_get(void **data, uint32_t *timestamp, buffer_ring_t *buf)
 {
-	pthread_mutex_lock(&buf->lock);
+	UniqueLock buffer_lock(buf->mutex);
 	// If there isn't a frame ready for us
-	while (!buf->valid)
-		pthread_cond_wait(&buf->cb_cond, &buf->lock);
+	while (!buf->valid) {
+		buf->cb_cond.wait(buffer_lock);
+	}
 	void *temp_buf = buf->bufs[0];
 	*data = buf->bufs[0] = buf->bufs[1];
 	buf->bufs[1] = temp_buf;
-	buf->valid = 0;
+	buf->valid = false;
 	*timestamp = buf->timestamp;
-	pthread_mutex_unlock(&buf->lock);
 	return 0;
 }
 
 
-/* 
+/*
   Use this to make sure the runloop is locked and no one is in it. Then you can
   call arbitrary functions from libfreenect.h in a safe way. If the kinect with
-  this index has not been initialized yet, then it will try to set it up. If 
+  this index has not been initialized yet, then it will try to set it up. If
   this function is successful, then you can access kinects[index]. Don't forget
   to unlock the runloop when you're done.
-  
+
   Returns 0 if successful, nonzero if kinect[index] is unvailable
- */ 
+ */
 static int runloop_enter(int index)
 {
 	if (index < 0 || index >= MAX_KINECTS) {
-		printf("Error: Invalid index [%d]\n", index);
+		FN_WARN_INVALID_INDEX(index);
 		return -1;
 	}
 	if (!thread_running || !kinects[index])
 		if (setup_kinect(index, FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT, 1))
 			return -1;
-		
+
 	pending_runloop_tasks_inc();
-	pthread_mutex_lock(&runloop_lock);
+	runloop_mutex.lock();
 	return 0;
 }
 
 static void runloop_exit()
 {
-	pthread_mutex_unlock(&runloop_lock);
+	runloop_mutex.unlock();
 	pending_runloop_tasks_dec();
 }
 
 int freenect_sync_get_video_with_res(void **video, uint32_t *timestamp, int index,
         freenect_resolution res, freenect_video_format fmt)
 {
 	if (index < 0 || index >= MAX_KINECTS) {
-		printf("Error: Invalid index [%d]\n", index);
+		FN_WARN_INVALID_INDEX(index);
 		return -1;
 	}
 	if (!thread_running || !kinects[index] || kinects[index]->video.fmt != fmt || kinects[index]->video.res != res)
@@ -384,7 +402,7 @@ int freenect_sync_get_depth_with_res(void **depth, uint32_t *timestamp, int inde
         freenect_resolution res, freenect_depth_format fmt)
 {
 	if (index < 0 || index >= MAX_KINECTS) {
-		printf("Error: Invalid index [%d]\n", index);
+		FN_WARN_INVALID_INDEX(index);
 		return -1;
 	}
 	if (!thread_running || !kinects[index] || kinects[index]->depth.fmt != fmt
@@ -404,7 +422,7 @@ int freenect_sync_get_tilt_state(freenect_raw_tilt_state **state, int index)
 {
 	if (runloop_enter(index)) return -1;
 	freenect_update_tilt_state(kinects[index]->dev);
-	*state = freenect_get_tilt_state(kinects[index]->dev);  
+	*state = freenect_get_tilt_state(kinects[index]->dev);
 	runloop_exit();
 	return 0;
 }
@@ -433,7 +451,7 @@ int freenect_sync_camera_to_world(int cx, int cy, int wz, double* wx, double* wy
 void freenect_sync_stop(void)
 {
 	if (thread_running) {
-		thread_running = 0;
-		pthread_join(thread, NULL);
+		thread_running = false;
+		thread.join();
 	}
 }