libfreenect_sync.c
Go to the documentation of this file.
1 /*
2  * This file is part of the OpenKinect Project. http://www.openkinect.org
3  *
4  * Copyright (c) 2010 Brandyn White (bwhite@dappervision.com)
5  * Andrew Miller (amiller@dappervision.com)
6  *
7  * This code is licensed to you under the terms of the Apache License, version
8  * 2.0, or, at your option, the terms of the GNU General Public License,
9  * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
10  * or the following URLs:
11  * http://www.apache.org/licenses/LICENSE-2.0
12  * http://www.gnu.org/licenses/gpl-2.0.txt
13  *
14  * If you redistribute this file in source form, modified or unmodified, you
15  * may:
16  * 1) Leave this header intact and distribute it under the same terms,
17  * accompanying it with the APACHE20 and GPL20 files, or
18  * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
19  * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file
20  * In all cases you must keep the copyright notice intact and include a copy
21  * of the CONTRIB file.
22  *
23  * Binary distributions must follow the binary distribution requirements of
24  * either License.
25  */
26 #include <stdio.h>
27 #include <pthread.h>
28 #include <string.h>
29 #include <stdlib.h>
30 #include <assert.h>
32 #include "libfreenect_sync.h"
33 
34 typedef struct buffer_ring {
35  pthread_mutex_t lock;
36  pthread_cond_t cb_cond;
37  void *bufs[3];
39  int valid; // True if middle buffer is valid
40  int fmt;
41  int res;
43 
44 typedef struct sync_kinect {
49 
50 typedef int (*set_buffer_t)(freenect_device *dev, void *buf);
51 
52 #define MAX_KINECTS 64
55 static int thread_running = 0;
56 static pthread_t thread;
57 static pthread_mutex_t runloop_lock = PTHREAD_MUTEX_INITIALIZER;
58 static int pending_runloop_tasks = 0;
59 static pthread_mutex_t pending_runloop_tasks_lock = PTHREAD_MUTEX_INITIALIZER;
60 static pthread_cond_t pending_runloop_tasks_cond = PTHREAD_COND_INITIALIZER;
61 
62 /* Locking Convention
63  Rules:
64  - if you need more than one lock on a line, get them from left to right
65  - do not mix locks on different lines
66  - if you need to change the lock rules, make sure you check everything and update this
67  Lock Families:
68  - pending_runloop_tasks_lock
69  - runloop_lock, buffer_ring_t.lock (NOTE: You may only have one)
70 */
71 
73 {
74  int sz, i;
75  switch (fmt) {
76  case FREENECT_VIDEO_RGB:
81  sz = freenect_find_video_mode(res, fmt).bytes;
82  break;
83  default:
84  printf("Invalid video format %d\n", fmt);
85  return -1;
86  }
87  for (i = 0; i < 3; ++i)
88  buf->bufs[i] = malloc(sz);
89  buf->timestamp = 0;
90  buf->valid = 0;
91  buf->fmt = fmt;
92  buf->res = res;
93  return 0;
94 }
95 
97 {
98  int sz, i;
99  switch (fmt) {
105  case FREENECT_DEPTH_MM:
106  sz = freenect_find_depth_mode(res, fmt).bytes;
107  break;
108  default:
109  printf("Invalid depth format %d\n", fmt);
110  return -1;
111  }
112  for (i = 0; i < 3; ++i)
113  buf->bufs[i] = malloc(sz);
114  buf->timestamp = 0;
115  buf->valid = 0;
116  buf->fmt = fmt;
117  buf->res = res;
118  return 0;
119 }
120 
122 {
123  int i;
124  for (i = 0; i < 3; ++i) {
125  free(buf->bufs[i]);
126  buf->bufs[i] = NULL;
127  }
128  buf->timestamp = 0;
129  buf->valid = 0;
130  buf->fmt = -1;
131  buf->res = -1;
132 }
133 
134 static void producer_cb_inner(freenect_device *dev, void *data, uint32_t timestamp, buffer_ring_t *buf, set_buffer_t set_buffer)
135 {
136  pthread_mutex_lock(&buf->lock);
137  assert(data == buf->bufs[2]);
138  void *temp_buf = buf->bufs[1];
139  buf->bufs[1] = buf->bufs[2];
140  buf->bufs[2] = temp_buf;
141  set_buffer(dev, temp_buf);
142  buf->timestamp = timestamp;
143  buf->valid = 1;
144  pthread_cond_signal(&buf->cb_cond);
145  pthread_mutex_unlock(&buf->lock);
146 }
147 
148 static void video_producer_cb(freenect_device *dev, void *data, uint32_t timestamp)
149 {
151 }
152 
153 static void depth_producer_cb(freenect_device *dev, void *data, uint32_t timestamp)
154 {
156 }
157 
158 /* You should only use these functions to manipulate the pending_runloop_tasks_lock*/
159 static void pending_runloop_tasks_inc(void)
160 {
161  pthread_mutex_lock(&pending_runloop_tasks_lock);
162  assert(pending_runloop_tasks >= 0);
164  pthread_mutex_unlock(&pending_runloop_tasks_lock);
165 }
166 
167 static void pending_runloop_tasks_dec(void)
168 {
169  pthread_mutex_lock(&pending_runloop_tasks_lock);
171  assert(pending_runloop_tasks >= 0);
173  pthread_cond_signal(&pending_runloop_tasks_cond);
174  pthread_mutex_unlock(&pending_runloop_tasks_lock);
175 }
176 
178 {
179  pthread_mutex_lock(&pending_runloop_tasks_lock);
180  while (pending_runloop_tasks)
182  pthread_mutex_unlock(&pending_runloop_tasks_lock);
183 }
184 
185 static void *init(void *unused)
186 {
188  pthread_mutex_lock(&runloop_lock);
189  while (thread_running && freenect_process_events(ctx) >= 0) {
190  pthread_mutex_unlock(&runloop_lock);
191  // NOTE: This lets you run tasks while process_events isn't running
193  pthread_mutex_lock(&runloop_lock);
194  }
195  // Go through each device, call stop video, close device
196  int i;
197  for (i = 0; i < MAX_KINECTS; ++i) {
198  if (kinects[i]) {
199  freenect_stop_video(kinects[i]->dev);
200  freenect_stop_depth(kinects[i]->dev);
201  freenect_set_user(kinects[i]->dev, NULL);
202  freenect_close_device(kinects[i]->dev);
203  free_buffer_ring(&kinects[i]->video);
204  free_buffer_ring(&kinects[i]->depth);
205  free(kinects[i]);
206  kinects[i] = NULL;
207  }
208  }
209  freenect_shutdown(ctx);
210  pthread_mutex_unlock(&runloop_lock);
211  return NULL;
212 }
213 
214 static void init_thread(void)
215 {
216  thread_running = 1;
217  freenect_init(&ctx, 0);
218  // We claim both the motor and the camera, because we can't know in advance
219  // which devices the caller will want, and the c_sync interface doesn't
220  // support audio, so there's no reason to claim the device needlessly.
222  pthread_create(&thread, NULL, init, NULL);
223 }
224 
226 {
227  freenect_stop_video(kinect->dev);
228  free_buffer_ring(&kinect->video);
229  if (alloc_buffer_ring_video(res, fmt, &kinect->video))
230  return -1;
232  freenect_set_video_buffer(kinect->dev, kinect->video.bufs[2]);
233  freenect_start_video(kinect->dev);
234  return 0;
235 }
236 
238 {
239  freenect_stop_depth(kinect->dev);
240  free_buffer_ring(&kinect->depth);
241  if (alloc_buffer_ring_depth(res, fmt, &kinect->depth))
242  return -1;
244  freenect_set_depth_buffer(kinect->dev, kinect->depth.bufs[2]);
245  freenect_start_depth(kinect->dev);
246  return 0;
247 }
248 
249 static sync_kinect_t *alloc_kinect(int index)
250 {
251  sync_kinect_t *kinect = (sync_kinect_t*)malloc(sizeof(sync_kinect_t));
252  if (freenect_open_device(ctx, &kinect->dev, index)) {
253  free(kinect);
254  return NULL;
255  }
256  int i;
257  for (i = 0; i < 3; ++i) {
258  kinect->video.bufs[i] = NULL;
259  kinect->depth.bufs[i] = NULL;
260  }
261  kinect->video.fmt = -1;
262  kinect->video.res = -1;
263  kinect->depth.fmt = -1;
264  kinect->depth.res = -1;
267  pthread_mutex_init(&kinect->video.lock, NULL);
268  pthread_mutex_init(&kinect->depth.lock, NULL);
269  pthread_cond_init(&kinect->video.cb_cond, NULL);
270  pthread_cond_init(&kinect->depth.cb_cond, NULL);
271  return kinect;
272 }
273 
274 static int setup_kinect(int index, int res, int fmt, int is_depth)
275 {
277  pthread_mutex_lock(&runloop_lock);
278  int thread_running_prev = thread_running;
279  if (!thread_running)
280  init_thread();
281  if (!kinects[index]) {
282  kinects[index] = alloc_kinect(index);
283  }
284  if (!kinects[index]) {
285  printf("Error: Invalid index [%d]\n", index);
286  // If we started the thread, we need to bring it back
287  if (!thread_running_prev) {
288  thread_running = 0;
289  pthread_mutex_unlock(&runloop_lock);
291  pthread_join(thread, NULL);
292  } else {
293  pthread_mutex_unlock(&runloop_lock);
295  }
296  return -1;
297  }
298  freenect_set_user(kinects[index]->dev, kinects[index]);
299  buffer_ring_t *buf;
300  if (is_depth)
301  buf = &kinects[index]->depth;
302  else
303  buf = &kinects[index]->video;
304  pthread_mutex_lock(&buf->lock);
305  if ((buf->fmt != fmt) || (buf->res != res))
306  {
307  if (is_depth)
309  else
311  }
312  pthread_mutex_unlock(&buf->lock);
313  pthread_mutex_unlock(&runloop_lock);
315  return 0;
316 }
317 
318 static int sync_get(void **data, uint32_t *timestamp, buffer_ring_t *buf)
319 {
320  pthread_mutex_lock(&buf->lock);
321  // If there isn't a frame ready for us
322  while (!buf->valid)
323  pthread_cond_wait(&buf->cb_cond, &buf->lock);
324  void *temp_buf = buf->bufs[0];
325  *data = buf->bufs[0] = buf->bufs[1];
326  buf->bufs[1] = temp_buf;
327  buf->valid = 0;
328  *timestamp = buf->timestamp;
329  pthread_mutex_unlock(&buf->lock);
330  return 0;
331 }
332 
333 
334 /*
335  Use this to make sure the runloop is locked and no one is in it. Then you can
336  call arbitrary functions from libfreenect.h in a safe way. If the kinect with
337  this index has not been initialized yet, then it will try to set it up. If
338  this function is successful, then you can access kinects[index]. Don't forget
339  to unlock the runloop when you're done.
340 
341  Returns 0 if successful, nonzero if kinect[index] is unvailable
342  */
343 static int runloop_enter(int index)
344 {
345  if (index < 0 || index >= MAX_KINECTS) {
346  printf("Error: Invalid index [%d]\n", index);
347  return -1;
348  }
349  if (!thread_running || !kinects[index])
351  return -1;
352 
354  pthread_mutex_lock(&runloop_lock);
355  return 0;
356 }
357 
358 static void runloop_exit()
359 {
360  pthread_mutex_unlock(&runloop_lock);
362 }
363 
366 {
367  if (index < 0 || index >= MAX_KINECTS) {
368  printf("Error: Invalid index [%d]\n", index);
369  return -1;
370  }
371  if (!thread_running || !kinects[index] || kinects[index]->video.fmt != fmt || kinects[index]->video.res != res)
372  if (setup_kinect(index, res, fmt, 0))
373  return -1;
374  sync_get(video, timestamp, &kinects[index]->video);
375  return 0;
376 }
377 
379 {
380  return freenect_sync_get_video_with_res(video, timestamp, index, FREENECT_RESOLUTION_MEDIUM, fmt);
381 }
382 
385 {
386  if (index < 0 || index >= MAX_KINECTS) {
387  printf("Error: Invalid index [%d]\n", index);
388  return -1;
389  }
390  if (!thread_running || !kinects[index] || kinects[index]->depth.fmt != fmt
391  || kinects[index]->depth.res != res)
392  if (setup_kinect(index, res, fmt, 1))
393  return -1;
394  sync_get(depth, timestamp, &kinects[index]->depth);
395  return 0;
396 }
397 
399 {
400  return freenect_sync_get_depth_with_res(depth, timestamp, index, FREENECT_RESOLUTION_MEDIUM, fmt);
401 }
402 
404 {
405  if (runloop_enter(index)) return -1;
406  freenect_update_tilt_state(kinects[index]->dev);
407  *state = freenect_get_tilt_state(kinects[index]->dev);
408  runloop_exit();
409  return 0;
410 }
411 
412 int freenect_sync_set_tilt_degs(int angle, int index) {
413  if (runloop_enter(index)) return -1;
414  freenect_set_tilt_degs(kinects[index]->dev, angle);
415  runloop_exit();
416  return 0;
417 }
418 
420  if (runloop_enter(index)) return -1;
421  freenect_set_led(kinects[index]->dev, led);
422  runloop_exit();
423  return 0;
424 }
425 
426 int freenect_sync_camera_to_world(int cx, int cy, int wz, double* wx, double* wy, int index) {
427  if (runloop_enter(index)) return -1;
428  freenect_camera_to_world(kinects[index]->dev, cx, cy, wz, wx, wy);
429  runloop_exit();
430  return 0;
431 }
432 
434 {
435  if (thread_running) {
436  thread_running = 0;
437  pthread_join(thread, NULL);
438  }
439 }
void freenect_set_video_callback(freenect_device *dev, freenect_video_cb cb)
Definition: fakenect.c:241
int freenect_sync_get_depth(void **depth, uint32_t *timestamp, int index, freenect_depth_format fmt)
int(* set_buffer_t)(freenect_device *dev, void *buf)
void freenect_select_subdevices(freenect_context *ctx, freenect_device_flags subdevs)
Definition: fakenect.c:338
int freenect_stop_video(freenect_device *dev)
Definition: fakenect.c:383
struct buffer_ring buffer_ring_t
freenect_video_format
Definition: libfreenect.h:86
static sync_kinect_t * kinects[MAX_KINECTS]
static int pending_runloop_tasks
static void runloop_exit()
struct sync_kinect sync_kinect_t
static void init_thread(void)
int freenect_start_depth(freenect_device *dev)
Definition: fakenect.c:365
int freenect_set_led(freenect_device *dev, freenect_led_options option)
Definition: fakenect.c:414
static int sync_get(void **data, uint32_t *timestamp, buffer_ring_t *buf)
int freenect_set_video_buffer(freenect_device *dev, void *buf)
Definition: fakenect.c:349
static sync_kinect_t * alloc_kinect(int index)
static pthread_mutex_t pending_runloop_tasks_lock
int freenect_shutdown(freenect_context *ctx)
Definition: fakenect.c:402
int freenect_sync_set_tilt_degs(int angle, int index)
static void pending_runloop_tasks_dec(void)
uint32_t timestamp
freenect_depth_format
Definition: libfreenect.h:99
int freenect_set_video_mode(freenect_device *dev, const freenect_frame_mode mode)
Definition: fakenect.c:246
int freenect_set_tilt_degs(freenect_device *dev, double angle)
Definition: fakenect.c:410
int freenect_sync_get_tilt_state(freenect_raw_tilt_state **state, int index)
void freenect_set_depth_callback(freenect_device *dev, freenect_depth_cb cb)
Definition: fakenect.c:236
#define MAX_KINECTS
static void producer_cb_inner(freenect_device *dev, void *data, uint32_t timestamp, buffer_ring_t *buf, set_buffer_t set_buffer)
int freenect_start_video(freenect_device *dev)
Definition: fakenect.c:371
freenect_frame_mode freenect_find_video_mode(freenect_resolution res, freenect_video_format fmt)
Definition: fakenect.c:260
static void free_buffer_ring(buffer_ring_t *buf)
void * freenect_get_user(freenect_device *dev)
Definition: fakenect.c:360
void * bufs[3]
freenect_frame_mode freenect_find_depth_mode(freenect_resolution res, freenect_depth_format fmt)
Definition: fakenect.c:284
static int thread_running
static pthread_t thread
freenect_led_options
Definition: libfreenect.h:148
pthread_cond_t cb_cond
void freenect_sync_stop(void)
static void video_producer_cb(freenect_device *dev, void *data, uint32_t timestamp)
freenect_raw_tilt_state * freenect_get_tilt_state(freenect_device *dev)
Definition: fakenect.c:217
freenect_device * dev
int freenect_sync_get_video_with_res(void **video, uint32_t *timestamp, int index, freenect_resolution res, freenect_video_format fmt)
static int change_depth_format(sync_kinect_t *kinect, freenect_resolution res, freenect_depth_format fmt)
int freenect_update_tilt_state(freenect_device *dev)
Definition: fakenect.c:418
static int alloc_buffer_ring_video(freenect_resolution res, freenect_video_format fmt, buffer_ring_t *buf)
static void * init(void *unused)
int freenect_close_device(freenect_device *dev)
Definition: fakenect.c:406
int freenect_set_depth_mode(freenect_device *dev, const freenect_frame_mode mode)
Definition: fakenect.c:253
buffer_ring_t video
int freenect_open_device(freenect_context *ctx, freenect_device **dev, int index)
Definition: fakenect.c:314
static pthread_mutex_t runloop_lock
freenect_resolution
Definition: libfreenect.h:77
static void pending_runloop_tasks_inc(void)
static freenect_context * ctx
int freenect_sync_get_depth_with_res(void **depth, uint32_t *timestamp, int index, freenect_resolution res, freenect_depth_format fmt)
Data from the tilt motor and accelerometer.
Definition: libfreenect.h:167
static void depth_producer_cb(freenect_device *dev, void *data, uint32_t timestamp)
int freenect_sync_camera_to_world(int cx, int cy, int wz, double *wx, double *wy, int index)
static int runloop_enter(int index)
unsigned int uint32_t
int freenect_sync_get_video(void **video, uint32_t *timestamp, int index, freenect_video_format fmt)
static pthread_cond_t pending_runloop_tasks_cond
capture state
Definition: micview.c:53
freenect_device_flags
Definition: libfreenect.h:58
pthread_mutex_t lock
int freenect_sync_set_led(freenect_led_options led, int index)
int freenect_stop_depth(freenect_device *dev)
Definition: fakenect.c:377
static int setup_kinect(int index, int res, int fmt, int is_depth)
int freenect_process_events(freenect_context *ctx)
Definition: fakenect.c:140
FREENECTAPI void freenect_camera_to_world(freenect_device *dev, int cx, int cy, int wz, double *wx, double *wy)
camera -> world coordinate helper function
Definition: registration.c:318
int freenect_set_depth_buffer(freenect_device *dev, void *buf)
Definition: fakenect.c:343
buffer_ring_t depth
static void pending_runloop_tasks_wait_zero(void)
void freenect_set_user(freenect_device *dev, void *user)
Definition: fakenect.c:355
static int alloc_buffer_ring_depth(freenect_resolution res, freenect_depth_format fmt, buffer_ring_t *buf)
int freenect_init(freenect_context **ctx, freenect_usb_context *usb_ctx)
Definition: fakenect.c:327
static int change_video_format(sync_kinect_t *kinect, freenect_resolution res, freenect_video_format fmt)


libfreenect
Author(s): Hector Martin, Josh Blake, Kyle Machulis, OpenKinect community
autogenerated on Thu Jun 6 2019 19:25:38