1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/kinect.cc	Sat Oct 05 16:03:08 2013 +0300
     1.3 @@ -0,0 +1,102 @@
     1.4 +#include <stdio.h>
     1.5 +#include <pthread.h>
     1.6 +#include <libfreenect.h>
     1.7 +#include "kinect.h"
     1.8 +
     1.9 +static void video_cb(freenect_device *kin_dev, void *rgb, uint32_t time);
    1.10 +static void depth_cb(freenect_device *kin_dev, void *depth, uint32_t time);
    1.11 +
    1.12 +static pthread_mutex_t video_mutex = PTHREAD_MUTEX_INITIALIZER;
    1.13 +static pthread_mutex_t depth_mutex = PTHREAD_MUTEX_INITIALIZER;
    1.14 +
    1.15 +bool init_kinect(freenect_context **kin_ctx, freenect_device **kin_dev, KinectParams *kin_params)
    1.16 +{
    1.17 +	if(freenect_init(kin_ctx, NULL) < 0) {
    1.18 +		fprintf(stderr, "Failed to initialize kinect context.\n");
    1.19 +		return false;
    1.20 +	}
    1.21 +
    1.22 +	freenect_set_log_level(*kin_ctx, FREENECT_LOG_INFO);
    1.23 +
    1.24 +	int num_dev = freenect_num_devices(*kin_ctx);
    1.25 +	if(num_dev < 1) {
    1.26 +		fprintf(stderr, "Kinect device not found.\n");
    1.27 +		return false;
    1.28 +	}
    1.29 +	printf("%d devices found, will use the first.\n", num_dev);
    1.30 +
    1.31 +	if(freenect_open_device(*kin_ctx, kin_dev, 0) < 0) {
    1.32 +		fprintf(stderr, "Couldn't open kinect device.\n");
    1.33 +		return false;
    1.34 +	}
    1.35 +
    1.36 +	if(freenect_set_tilt_degs(*kin_dev, kin_params->angle) < 0) {
    1.37 +		stop_kinect(*kin_ctx, *kin_dev);
    1.38 +		fprintf(stderr, "Failed to set kinect angle\n");
    1.39 +		return false;
    1.40 +	}
    1.41 +
    1.42 +	if(freenect_set_led(*kin_dev, kin_params->led_color) < 0) {
    1.43 +		stop_kinect(*kin_ctx, *kin_dev);
    1.44 +		fprintf(stderr, "Failed to set kinect LED\n");
    1.45 +		return false;
    1.46 +	}
    1.47 +
    1.48 +	pthread_mutex_init(&video_mutex, NULL);
    1.49 +	pthread_mutex_init(&depth_mutex, NULL);
    1.50 +
    1.51 +	freenect_set_video_callback(*kin_dev, video_cb);
    1.52 +	freenect_set_depth_callback(*kin_dev, depth_cb);
    1.53 +
    1.54 +	if(freenect_set_video_mode(*kin_dev,
    1.55 +			freenect_find_video_mode(kin_params->video_res,
    1.56 +				kin_params->video_format)) < 0) {
    1.57 +		stop_kinect(*kin_ctx, *kin_dev);
    1.58 +		fprintf(stderr, "Failed to set kinect video mode.\n");
    1.59 +		return false;
    1.60 +	}
    1.61 +
    1.62 +	if(freenect_set_depth_mode(*kin_dev,
    1.63 +				freenect_find_depth_mode(kin_params->depth_res,
    1.64 +					kin_params->depth_format)) < 0) {
    1.65 +		stop_kinect(*kin_ctx, *kin_dev);
    1.66 +		fprintf(stderr, "Failed to set kinect depth mode.\n");
    1.67 +		return false;
    1.68 +	}
    1.69 +
    1.70 +	if(freenect_start_video(*kin_dev) < 0) {
    1.71 +		fprintf(stderr, "Failed to start kinect video.\n");
    1.72 +		return false;
    1.73 +	}
    1.74 +
    1.75 +	if(freenect_start_depth(*kin_dev) < 0) {
    1.76 +		fprintf(stderr, "Failed to start kinect depth video.\n");
    1.77 +		return false;
    1.78 +	}
    1.79 +
    1.80 +	return true;
    1.81 +}
    1.82 +
    1.83 +void stop_kinect(freenect_context *kin_ctx, freenect_device *kin_dev)
    1.84 +{
    1.85 +	freenect_set_led(kin_dev, LED_OFF);
    1.86 +
    1.87 +	freenect_stop_depth(kin_dev);
    1.88 +	freenect_stop_video(kin_dev);
    1.89 +	freenect_close_device(kin_dev);
    1.90 +	freenect_shutdown(kin_ctx);
    1.91 +}
    1.92 +
    1.93 +void video_cb(freenect_device *kin_dev, void *rgb, uint32_t time)
    1.94 +{
    1.95 +	pthread_mutex_lock(&video_mutex);
    1.96 +	printf("Started video.\n");
    1.97 +	pthread_mutex_unlock(&video_mutex);
    1.98 +}
    1.99 +
   1.100 +void depth_cb(freenect_device *kin_dev, void *depth, uint32_t time)
   1.101 +{
   1.102 +	pthread_mutex_lock(&depth_mutex);
   1.103 +	printf("Started depth.\n");
   1.104 +	pthread_mutex_unlock(&depth_mutex);
   1.105 +}