From adbfb27a90dacca1cdea40521d8614f61fe3f7cb Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Sat, 19 Mar 2016 16:48:54 -0400 Subject: roll my own mpjpeg encoding (with libjpeg) --- Makefile | 4 +- freenect-server--kinect.c | 223 -------------------------------------- freenect-server-http.socket | 9 ++ freenect-server.c | 256 ++++++++++++++++++++++++++++++++++++++++++++ freenect-server.service | 4 +- freenect-server.sh | 25 ----- freenect-server.socket | 8 -- freenect-server@.socket | 13 +++ 8 files changed, 282 insertions(+), 260 deletions(-) delete mode 100644 freenect-server--kinect.c create mode 100644 freenect-server-http.socket create mode 100644 freenect-server.c delete mode 100644 freenect-server.sh delete mode 100644 freenect-server.socket create mode 100644 freenect-server@.socket diff --git a/Makefile b/Makefile index 5ae2d4b..f9df186 100644 --- a/Makefile +++ b/Makefile @@ -4,10 +4,10 @@ CFLAGS += -g -std=c99 -Wall -Werror -Wextra -pedantic CPPFLAGS += -std=c99 -Wall -Werror -Wextra -pedantic CPPFLAGS += -D_GNU_SOURCE -all: freenect-server freenect-server--kinect multipart-replace-http-server +all: freenect-server multipart-replace-http-server .PHONY: all -freenect-server--kinect: util.o -lfreenect -lusb-1.0 +freenect-server: util.o -lfreenect -lusb-1.0 -ljpeg multipart-replace-http-server: util.o multipart-replace.o -lpthread clean: diff --git a/freenect-server--kinect.c b/freenect-server--kinect.c deleted file mode 100644 index e6c9e7e..0000000 --- a/freenect-server--kinect.c +++ /dev/null @@ -1,223 +0,0 @@ -/* - * 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); -} diff --git a/freenect-server-http.socket b/freenect-server-http.socket new file mode 100644 index 0000000..3fc6244 --- /dev/null +++ b/freenect-server-http.socket @@ -0,0 +1,9 @@ +[Unit] +Description=Kinect HTTP media streamer socket + +[Socket] +ListenStream=5800 + +[Install] +WantedBy=sockets.target +Also=freenect-server@accel.mjson.socket freenect-server@depth.mjpg.socket freenect-server@video.mjpg.socket diff --git a/freenect-server.c b/freenect-server.c new file mode 100644 index 0000000..37e2a08 --- /dev/null +++ b/freenect-server.c @@ -0,0 +1,256 @@ +/* + * 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 +#include /* atexit */ +#include /* dup2 */ + +#include +#include + +#include + +#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 != 460*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 != 460*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() { + 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); +} diff --git a/freenect-server.service b/freenect-server.service index 5a2d4e2..fbc2035 100644 --- a/freenect-server.service +++ b/freenect-server.service @@ -1,7 +1,7 @@ [Unit] -Description=Kinect media streamer +Description=Kinect media streamer backend After=network.target -Requires=freenect-server.socket +Requires=freenect-server@accel.socket freenect-server@depth.socket freenect-server@video.socket [Service] Type=simple diff --git a/freenect-server.sh b/freenect-server.sh deleted file mode 100644 index e88acbe..0000000 --- a/freenect-server.sh +++ /dev/null @@ -1,25 +0,0 @@ -#!/usr/bin/env bash -# Copyright 2016 Luke Shumaker - -PATH="$PATH:$(dirname -- "$0")" - -t="$(mktemp -d --tmpdir "${0##*/}.XXXXXXXXXX")" - -mkfifo $t/video.rgb24 -mkfifo $t/depth.rgb24 -mkfifo $t/video.mjpg -mkfifo $t/depth.mjpg -mkfifo $t/accel.mjson - -( freenect-server--kinect -v $t/video.rgb24 -d $t/depth.rgb24 -a $t/accel.mjson; echo "EXITED: freenect-server--kinect: $?") & pids+=($!) -( multipart-replace-http-server "$1" "$2" $t/video.mjpg $t/depth.mjpg $t/accel.mjson; echo "EXITED: freenect-server--http: $?") & pids+=($!) -( ffmpeg -loglevel warning -pixel_format rgb24 -s 640x480 -f rawvideo -i $t/video.rgb24 -q:v 1 -f mpjpeg - > $t/video.mjpg; echo "EXITED: ffmpeg video: $?") & pids+=($!) -( ffmpeg -loglevel warning -pixel_format rgb24 -s 640x480 -f rawvideo -i $t/depth.rgb24 -q:v 1 -f mpjpeg - > $t/depth.mjpg; echo "EXITED: ffmpeg depth: $?") & pids+=($!) - -cleanup() { - kill -- "${pids[@]}" 2>/dev/null - rm -rf -- "$t" -} -trap cleanup EXIT - -wait diff --git a/freenect-server.socket b/freenect-server.socket deleted file mode 100644 index 4b98eca..0000000 --- a/freenect-server.socket +++ /dev/null @@ -1,8 +0,0 @@ -[Unit] -Description=Kinect media streamer socket - -[Socket] -ListenStream=5800 - -[Install] -WantedBy=sockets.target diff --git a/freenect-server@.socket b/freenect-server@.socket new file mode 100644 index 0000000..841f2fd --- /dev/null +++ b/freenect-server@.socket @@ -0,0 +1,13 @@ +[Unit] +Description=Kinect media streamer internal %I stream +After=network.target + +[Socket] +SocketUser=alarm +SocketGroup=alarm +ListenFIFO=/run/freenect-server/Ii.fifo +FileDescriptorName=%I +Service=freenect-server.service + +[Install] +WantedBy=sockets.target -- cgit v1.2.3