invisible
view src/kinect.cc @ 0:80df8030105b
init kinect
| author | Eleni Maria Stea <eleni@mutantstargoat.com> |
|---|---|
| date | Sat, 05 Oct 2013 16:03:08 +0300 |
| parents | |
| children | fdbd55eaa14e |
line source
1 #include <stdio.h>
2 #include <pthread.h>
3 #include <libfreenect.h>
4 #include "kinect.h"
6 static void video_cb(freenect_device *kin_dev, void *rgb, uint32_t time);
7 static void depth_cb(freenect_device *kin_dev, void *depth, uint32_t time);
9 static pthread_mutex_t video_mutex = PTHREAD_MUTEX_INITIALIZER;
10 static pthread_mutex_t depth_mutex = PTHREAD_MUTEX_INITIALIZER;
12 bool init_kinect(freenect_context **kin_ctx, freenect_device **kin_dev, KinectParams *kin_params)
13 {
14 if(freenect_init(kin_ctx, NULL) < 0) {
15 fprintf(stderr, "Failed to initialize kinect context.\n");
16 return false;
17 }
19 freenect_set_log_level(*kin_ctx, FREENECT_LOG_INFO);
21 int num_dev = freenect_num_devices(*kin_ctx);
22 if(num_dev < 1) {
23 fprintf(stderr, "Kinect device not found.\n");
24 return false;
25 }
26 printf("%d devices found, will use the first.\n", num_dev);
28 if(freenect_open_device(*kin_ctx, kin_dev, 0) < 0) {
29 fprintf(stderr, "Couldn't open kinect device.\n");
30 return false;
31 }
33 if(freenect_set_tilt_degs(*kin_dev, kin_params->angle) < 0) {
34 stop_kinect(*kin_ctx, *kin_dev);
35 fprintf(stderr, "Failed to set kinect angle\n");
36 return false;
37 }
39 if(freenect_set_led(*kin_dev, kin_params->led_color) < 0) {
40 stop_kinect(*kin_ctx, *kin_dev);
41 fprintf(stderr, "Failed to set kinect LED\n");
42 return false;
43 }
45 pthread_mutex_init(&video_mutex, NULL);
46 pthread_mutex_init(&depth_mutex, NULL);
48 freenect_set_video_callback(*kin_dev, video_cb);
49 freenect_set_depth_callback(*kin_dev, depth_cb);
51 if(freenect_set_video_mode(*kin_dev,
52 freenect_find_video_mode(kin_params->video_res,
53 kin_params->video_format)) < 0) {
54 stop_kinect(*kin_ctx, *kin_dev);
55 fprintf(stderr, "Failed to set kinect video mode.\n");
56 return false;
57 }
59 if(freenect_set_depth_mode(*kin_dev,
60 freenect_find_depth_mode(kin_params->depth_res,
61 kin_params->depth_format)) < 0) {
62 stop_kinect(*kin_ctx, *kin_dev);
63 fprintf(stderr, "Failed to set kinect depth mode.\n");
64 return false;
65 }
67 if(freenect_start_video(*kin_dev) < 0) {
68 fprintf(stderr, "Failed to start kinect video.\n");
69 return false;
70 }
72 if(freenect_start_depth(*kin_dev) < 0) {
73 fprintf(stderr, "Failed to start kinect depth video.\n");
74 return false;
75 }
77 return true;
78 }
80 void stop_kinect(freenect_context *kin_ctx, freenect_device *kin_dev)
81 {
82 freenect_set_led(kin_dev, LED_OFF);
84 freenect_stop_depth(kin_dev);
85 freenect_stop_video(kin_dev);
86 freenect_close_device(kin_dev);
87 freenect_shutdown(kin_ctx);
88 }
90 void video_cb(freenect_device *kin_dev, void *rgb, uint32_t time)
91 {
92 pthread_mutex_lock(&video_mutex);
93 printf("Started video.\n");
94 pthread_mutex_unlock(&video_mutex);
95 }
97 void depth_cb(freenect_device *kin_dev, void *depth, uint32_t time)
98 {
99 pthread_mutex_lock(&depth_mutex);
100 printf("Started depth.\n");
101 pthread_mutex_unlock(&depth_mutex);
102 }