diff options
Diffstat (limited to 'freenect-server--kinect.c')
| -rw-r--r-- | freenect-server--kinect.c | 165 |
1 files changed, 165 insertions, 0 deletions
diff --git a/freenect-server--kinect.c b/freenect-server--kinect.c new file mode 100644 index 0000000..1c68fe3 --- /dev/null +++ b/freenect-server--kinect.c @@ -0,0 +1,165 @@ +#include <errno.h> +#include <error.h> +#include <stdio.h> +#include <stdlib.h> /* atexit */ +#include <unistd.h> /* dup2 */ + +#include <libfreenect/libfreenect.h> +#include <libusb-1.0/libusb.h> + +#include "util.h" + +FILE *depth_stream = NULL; +FILE *video_stream = NULL; +FILE *accel_stream = NULL; +freenect_context *ctx; + +void dump_ffmpeg_24(FILE *stream, uint32_t timestamp UNUSED, void *data, + size_t data_size) +{ + fwrite(data, data_size, 1, stream); +} + +void dump_ffmpeg_pad16(FILE *stream, uint32_t timestamp UNUSED, void *data_anon, + size_t data_size) +{ + uint16_t* data = data_anon; + uint16_t* end = (void*)&((char*)data_anon)[data_size]; + while (data < end) { + uint32_t z = *data; + fwrite(((char*)(&z)), 3, 1, stream); + data = &data[1]; + } +} + +void handle_accel(freenect_device *dev UNUSED, freenect_raw_tilt_state* data) +{ + printf("handle accel\n"); + double x, y, z; + freenect_get_mks_accel(data, &x, &y, &z); + //fprintf(accel_stream, "x=%f\ty=%f\tz=%f\n", x, y, z); +} + +void handle_depth(freenect_device *dev UNUSED, void *depth, uint32_t timestamp) +{ + printf("handle depth\n"); + dump_ffmpeg_pad16(depth_stream, timestamp, depth, + freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, + FREENECT_DEPTH_11BIT).bytes); +} + +void handle_video(freenect_device *dev, void *rgb, uint32_t timestamp) +{ + printf("handle video\n"); + dump_ffmpeg_24(video_stream, timestamp, rgb, + freenect_get_current_video_mode(dev).bytes); +} + +void print_mode(const char *name, freenect_frame_mode mode) { + /* This is just a courtesy function to let the user know the mode + if it becomes a bother for maintainability just comment out the + code in its body. It will only break if struct entries go missing. + */ + printf("%s Mode: {%d, %d, {%d}, %d, %d, %d, %d, %d, %d, %d}\n", name, + mode.reserved, (int)mode.resolution, (int)mode.video_format, mode.bytes, mode.width, + mode.height, mode.data_bits_per_pixel, mode.padding_bits_per_pixel, + mode.framerate, mode.is_valid); +} + +void cleanup() { + if (ctx) + freenect_shutdown(ctx); + if (video_stream) + fclose(video_stream); + if (depth_stream) + fclose(depth_stream); + if (accel_stream) + fclose(accel_stream); +} + +void usage() { + printf("Usage: %s [-h] [-v video.rgb24] [-d depth.rgb24] [-a accel.txt]\n", program_invocation_name); +} + +int main(int argc, char *argv[]) { + int res = 0; + freenect_device *dev; + + atexit(cleanup); + + for (int i = 1; i < argc; i++) { + if (argv[i][0] == '-' && argv[i][1] != '\0' && argv[i][2] == '\0') { + switch (argv[i][1]) { + case 'h': + usage(); + return EXIT_SUCCESS; + case 'v': + i++; + video_stream = fopen(argv[i], "w"); + break; + case 'd': + i++; + depth_stream = fopen(argv[i], "w"); + break; + case 'a': + i++; + accel_stream = fopen(argv[i], "w"); + break; + default: + dup2(2, 1); + usage(); + return EXIT_FAILURE; + } + } else { + dup2(2, 1); + usage(); + return EXIT_FAILURE; + } + } + + res = freenect_init(&ctx, 0); + if (res) { + error(EXIT_FAILURE, 0, "freenect_init: %s", libusb_strerror(res)); + } + + freenect_device_flags devices = 0; + if (video_stream || depth_stream) + devices |= FREENECT_DEVICE_CAMERA; + if (accel_stream) + devices |= FREENECT_DEVICE_MOTOR; + + freenect_select_subdevices(ctx, devices); + if ((res = freenect_open_device(ctx, &dev, 0))) { + error(EXIT_FAILURE, 0, "freenect_open_device: %s", libusb_strerror(res)); + } + + if (video_stream) { + print_mode("Video", freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB)); + freenect_set_video_mode(dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB)); + freenect_start_video(dev); + freenect_set_video_callback(dev, handle_video); + } + + if (depth_stream) { + print_mode("Depth", freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT)); + freenect_set_depth_mode(dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT)); + freenect_start_depth(dev); + freenect_set_depth_callback(dev, handle_depth); + } + + if (accel_stream) { + } + + while (freenect_process_events(ctx) >= 0) { + if (accel_stream) { + freenect_raw_tilt_state* state; + freenect_update_tilt_state(dev); + state = freenect_get_tilt_state(dev); + handle_accel(dev, state); + } + } + + freenect_stop_depth(dev); + freenect_stop_video(dev); + freenect_close_device(dev); +} |