diff options
Diffstat (limited to 'src/freenect-server.c')
| -rw-r--r-- | src/freenect-server.c | 289 |
1 files changed, 289 insertions, 0 deletions
diff --git a/src/freenect-server.c b/src/freenect-server.c new file mode 100644 index 0000000..c5ed99a --- /dev/null +++ b/src/freenect-server.c @@ -0,0 +1,289 @@ +/* + * 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 <errno.h> +#include <error.h> +#include <inttypes.h> +#include <stdio.h> +#include <stdlib.h> /* atexit */ +#include <unistd.h> /* dup2 */ + +#include <libfreenect/libfreenect.h> +#include <libusb-1.0/libusb.h> + +#include <jpeglib.h> + +#include "util.h" + +FILE *depth_stream = NULL; +FILE *video_stream = NULL; +FILE *accel_stream = NULL; +freenect_context *ctx; + +struct jpeg_error_mgr jpeg_encoder_err; +struct jpeg_compress_struct jpeg_encoder; +unsigned char *jpeg_encoder_buf; +unsigned long jpeg_encoder_len; + +void jpeg_init() { + jpeg_encoder.err = jpeg_std_error(&jpeg_encoder_err); + jpeg_create_compress(&jpeg_encoder); + jpeg_encoder.image_width = 640; + jpeg_encoder.image_height = 480; + jpeg_encoder.input_components = 3; + jpeg_encoder.in_color_space = JCS_RGB; + jpeg_set_defaults(&jpeg_encoder); + jpeg_mem_dest(&jpeg_encoder, &jpeg_encoder_buf, &jpeg_encoder_len); +} + +void write_imageframe(FILE *stream, JSAMPLE *data) { + JSAMPLE *row_pointers[480]; + for (size_t i = 0; i < 480; i++) + row_pointers[i] = &data[i*640*3]; + + jpeg_start_compress(&jpeg_encoder, TRUE); + while (jpeg_encoder.next_scanline < jpeg_encoder.image_height) + jpeg_write_scanlines(&jpeg_encoder, &row_pointers[jpeg_encoder.next_scanline], 480-jpeg_encoder.next_scanline); + jpeg_finish_compress(&jpeg_encoder); + + if (fprintf(stream, + "--ffserver\r\n" + "Content-type: image/jpeg\r\n" + "Content-length: %lu\r\n" + "\r\n", + jpeg_encoder_len) < 1) + error(EXIT_FAILURE, ferror(stream), "writing header"); + if (fwrite(jpeg_encoder_buf, jpeg_encoder_len, 1, stream) < 1) + error(EXIT_FAILURE, ferror(stream), "writing body"); + if (fwrite("\r\n", 2, 1, stream) < 1) + error(EXIT_FAILURE, ferror(stream), "writing footer"); + fflush(stream); +} + +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"; + break; + case TILT_STATUS_LIMIT: + motor = "limit"; + break; + case TILT_STATUS_MOVING: + motor = "moving"; + break; + 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); + fflush(accel_stream); +} + +uint8_t depth_rgb[640*480*3]; +void handle_depth(freenect_device *dev, void *depth, uint32_t timestamp UNUSED) { + uint32_t size = freenect_get_current_depth_mode(dev).bytes; + if (size != 640*480*2) + error(EXIT_FAILURE, 0, "handle_depth: expected 640*480*2 byte frame, but got %"PRId32, size); + /* scale the 11-bit values into 8-bit values */ + uint16_t *depth_grey = depth; + for (size_t i = 0; i < 640*480; i++) + depth_rgb[i*3+0] = depth_rgb[i*3+1] = depth_rgb[i*3+2] = (uint8_t)(depth_grey[i]*8.0/11.0); + /* write the image */ + write_imageframe(depth_stream, depth_rgb); +} + +void handle_video(freenect_device *dev, void *rgb, uint32_t timestamp UNUSED) { + uint32_t size = freenect_get_current_video_mode(dev).bytes; + if (size != 640*480*3) + error(EXIT_FAILURE, 0, "handle_video: expected 640*480*3 byte frame, but got %"PRId32, size); + /* write the image */ + write_imageframe(video_stream, rgb); +} + +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() { + log("STOPPING=1"); + if (ctx) + freenect_shutdown(ctx); + if (video_stream) + fclose(video_stream); + if (depth_stream) + fclose(depth_stream); + if (accel_stream) + fclose(accel_stream); + fflush(stderr); + sleep(5); /* work around systemd bug dropping log messages */ +} + +FILE *xfopen(const char *path, const char *mode) { + FILE *stream = fopen(path, mode); + if (stream == NULL) + error(EXIT_FAILURE, errno, "fopen: %s", path); + return stream; +} + +FILE *xfdopen(const char *path, const char *mode) { + int fd = get_fd(path); + if (fd < 0) + error(EXIT_FAILURE, -fd, "get_fd: %s", path); + FILE *stream = fdopen(fd, mode); + if (stream == NULL) + error(EXIT_FAILURE, errno, "fdopen: %s (%d)", path, fd); + return stream; +} + +void usage() { + printf("Usage: %s [-h] [-v video.mjpg|-V video_fd] [-d depth.mjpg|-D depth_fd] [-a accel.mjson|-A accel_fd]\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 = xfopen(argv[i], "w"); + break; + case 'd': + i++; + depth_stream = xfopen(argv[i], "w"); + break; + case 'a': + i++; + accel_stream = xfopen(argv[i], "w"); + break; + case 'V': + i++; + video_stream = xfdopen(argv[i], "w"); + break; + case 'D': + i++; + depth_stream = xfdopen(argv[i], "w"); + break; + case 'A': + i++; + accel_stream = xfdopen(argv[i], "w"); + break; + default: + dup2(2, 1); + usage(); + return EXIT_FAILURE; + } + } else { + dup2(2, 1); + usage(); + return EXIT_FAILURE; + } + } + if (!(video_stream || depth_stream || accel_stream)) { + dup2(2, 1); + usage(); + return EXIT_FAILURE; + } + + jpeg_init(); + + 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); +} |