/* * This file is part of the OpenKinect Project. http://www.openkinect.org * * Copyright (c) 2010 Brandyn White (bwhite@dappervision.com) * Copyright (c) 2016 Luke Shumaker (lukeshu@sbcglobal.net) * * This code is licensed to you under the terms of the Apache License, version * 2.0, or, at your option, the terms of the GNU General Public License, * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, * or the following URLs: * http://www.apache.org/licenses/LICENSE-2.0 * http://www.gnu.org/licenses/gpl-2.0.txt * * If you redistribute this file in source form, modified or unmodified, you * may: * 1) Leave this header intact and distribute it under the same terms, * accompanying it with the APACHE20 and GPL20 files, or * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file * In all cases you must keep the copyright notice intact and include a copy * of the CONTRIB file. * * Binary distributions must follow the binary distribution requirements of * either License. */ #include #include #include #include /* atexit */ #include /* dup2 */ #include #include #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) { uint16_t z = *data; uint8_t out[3]; memset(out, (uint8_t)(z*2.0/3.0), 3); fwrite(out, 3, 1, stream); data = &data[1]; } } void handle_accel(freenect_device *dev UNUSED, freenect_raw_tilt_state* data) { double x, y, z, angle; freenect_get_mks_accel(data, &x, &y, &z); angle = freenect_get_tilt_degs(data); const char *motor; switch (data->tilt_status) { case TILT_STATUS_STOPPED: motor = "stopped"; case TILT_STATUS_LIMIT: motor = "limit"; case TILT_STATUS_MOVING: motor = "moving"; default: motor = "unknown"; } char *json = NULL; int len = asprintf(&json, "{ \"x\": %f,\n" " \"y\": %f,\n" " \"z\": %f,\n" " \"angle\": %f,\n" " \"motor\": \"%s\" }\n", x, y, z, angle, motor); if (len < 0) error(EXIT_FAILURE, errno, "asprintf"); fprintf(accel_stream, "--ffserver\r\n" "Content-type: application/json\r\n" "Content-length: %d\r\n" "\r\n" "%s\r\n", len, json); } void handle_depth(freenect_device *dev UNUSED, void *depth, uint32_t timestamp) { 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) { 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"); if (video_stream == NULL) error(EXIT_FAILURE, errno, "fopen: %s", argv[i]); break; case 'd': i++; depth_stream = fopen(argv[i], "w"); if (video_stream == NULL) error(EXIT_FAILURE, errno, "fopen: %s", argv[i]); break; case 'a': i++; accel_stream = fopen(argv[i], "w"); if (video_stream == NULL) error(EXIT_FAILURE, errno, "fopen: %s", argv[i]); 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); } 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); }