From 7582cc362b42687d02df07be7ff863119d6e55b2 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Tue, 30 May 2017 21:30:23 -0400 Subject: [PATCH] c_sync: using std::thread; Signed-off-by: Benn Snyder --- wrappers/c_sync/CMakeLists.txt | 11 +- ...ibfreenect_sync.c => libfreenect_sync.cpp} | 236 ++++++++++-------- 2 files changed, 134 insertions(+), 113 deletions(-) rename wrappers/c_sync/{libfreenect_sync.c => libfreenect_sync.cpp} (72%) diff --git a/wrappers/c_sync/CMakeLists.txt b/wrappers/c_sync/CMakeLists.txt index bc93bb85..37a2922a 100644 --- a/wrappers/c_sync/CMakeLists.txt +++ b/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 diff --git a/wrappers/c_sync/libfreenect_sync.c b/wrappers/c_sync/libfreenect_sync.cpp similarity index 72% rename from wrappers/c_sync/libfreenect_sync.c rename to wrappers/c_sync/libfreenect_sync.cpp index 1b2e0494..9ad1533d 100644 --- a/wrappers/c_sync/libfreenect_sync.c +++ b/wrappers/c_sync/libfreenect_sync.cpp @@ -23,20 +23,37 @@ * Binary distributions must follow the binary distribution requirements of * either License. */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include "libfreenect_registration.h" #include "libfreenect_sync.h" +using std::cerr; +using std::endl; +using Mutex = std::mutex; +using UniqueLock = std::unique_lock; +using LockGuard = std::lock_guard; +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,47 +64,59 @@ 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; @@ -95,7 +124,7 @@ static int alloc_buffer_ring_video(freenect_resolution res, freenect_video_forma 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: @@ -103,16 +132,17 @@ static int alloc_buffer_ring_depth(freenect_resolution res, freenect_depth_forma 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; @@ -120,29 +150,27 @@ static int alloc_buffer_ring_depth(freenect_resolution res, freenect_depth_forma 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,15 +274,14 @@ 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; @@ -264,44 +289,38 @@ static sync_kinect_t *alloc_kinect(int index) 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) @@ -309,55 +328,54 @@ static int setup_kinect(int index, int res, int fmt, int is_depth) 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(); } @@ -365,7 +383,7 @@ int freenect_sync_get_video_with_res(void **video, uint32_t *timestamp, int inde 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(); } }