invisible

annotate src/kinect.cc @ 0:80df8030105b

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