From 28887d5145e41c9e073c7c6e6990c4e218e05628 Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Fri, 11 Mar 2016 13:57:50 -0500 Subject: stuff --- .gitignore | 4 ++ Makefile | 19 +++++ main.c | 218 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ main.h | 39 ++++++++++ thread_http.c | 124 ++++++++++++++++++++++++++++++++ thread_kinect.c | 121 +++++++++++++++++++++++++++++++ thread_mpjpeg.c | 90 +++++++++++++++++++++++ wg.c | 43 +++++++++++ wg.h | 19 +++++ 9 files changed, 677 insertions(+) create mode 100644 .gitignore create mode 100644 Makefile create mode 100644 main.c create mode 100644 main.h create mode 100644 thread_http.c create mode 100644 thread_kinect.c create mode 100644 thread_mpjpeg.c create mode 100644 wg.c create mode 100644 wg.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4d63b10 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +/freenect-server + +*.o +.*.mk \ No newline at end of file diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..39e1290 --- /dev/null +++ b/Makefile @@ -0,0 +1,19 @@ +CFLAGS += -g -std=c99 -Wall -Werror -Wextra -pedantic +CPPFLAGS += -std=c99 -Wall -Werror -Wextra -pedantic +CPPFLAGS += -D_GNU_SOURCE + +all: freenect-server +.PHONY: all + +freenect-server: main.o thread_kinect.o thread_mpjpeg.o thread_http.o wg.o -lfreenect -lusb-1.0 -lpthread + $(CC) $(LDFLAGS) $^ -o $@ + +clean: + rm -f -- *.o .*.mk freenect-server +.PHONY: clean + +.DELETE_ON_ERROR: +.SECONDARY: + +CPPFLAGS += -MD -MF ${@:%.o=.%.mk} -MP +-include .*.mk diff --git a/main.c b/main.c new file mode 100644 index 0000000..0745df9 --- /dev/null +++ b/main.c @@ -0,0 +1,218 @@ +/* + * 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 +#include +#include +#include + +#include "main.h" +#include "wg.h" + +volatile sig_atomic_t running = 1; +static struct wg wg; + +void * xrealloc(void *ptr, size_t size) +{ + void *ret = realloc(ptr, size); + if (ret == NULL) { + if (ptr==NULL) + error(1, errno, "Could not allocate memory"); + else + error(1, errno, "Could not re-allocate memory"); + } + return ret; +} + +static +void start_ffmpeg(int in, int out) { + switch (fork()) { + case -1: /* error */ + error(1, errno, "fork"); + case 0: /* child */ + dup2(in, 0); + dup2(out, 1); + for (int fd = 3; fd < 30; fd++) + close(fd); + execlp("ffmpeg", + "ffmpeg", + "-pix_fmt", "rgb24", "-s", "640x480", "-f", "rawvideo", "-i", "-", + "-f", "mpjpeg", "-", + NULL); + error(1, errno, "execlp: ffmpeg"); + default: /* parent */ + close(in); + close(out); + } +} + +struct thread_kinect_args { + int video_fd; + int depth_fd; + int accel_fd; +}; +static +void* start_kinect_inner(void *args_anon) { + pthread_setname_np(pthread_self(), "kinect"); + struct thread_kinect_args *args = args_anon; + thread_kinect(args->video_fd, args->depth_fd, args->accel_fd); + wg_sub(&wg, 1); + running = 0; + return NULL; +} +static +void start_kinect(int video_fd, int depth_fd, int accel_fd) { + + static struct thread_kinect_args args; + args.video_fd = video_fd; + args.depth_fd = depth_fd; + args.accel_fd = accel_fd; + + static pthread_t thread; + wg_add(&wg, 1); + pthread_create(&thread, NULL, start_kinect_inner, &args); +} + +struct thread_mpjpeg_args { + struct mpjpeg_stream *s; + int fd; + const char *boundary; +}; +static +void *start_mpjpeg_reader_inner(void *args_anon) { + pthread_setname_np(pthread_self(), "mpjpeg-reader"); + struct thread_mpjpeg_args *args = args_anon; + thread_mpjpeg_reader(args->s, args->fd, args->boundary); + wg_sub(&wg, 1); + return NULL; +} +static +void start_mpjpeg_reader(struct mpjpeg_stream *s, int fd, const char *boundary) { + struct thread_mpjpeg { + struct thread_mpjpeg_args args; + pthread_t thread; + }; + struct thread_mpjpeg *thread = xrealloc(NULL, sizeof(struct thread_mpjpeg_args)); + thread->args.s = s; + thread->args.fd = fd; + thread->args.boundary = boundary; + wg_add(&wg, 1); + pthread_create(&thread->thread, NULL, start_mpjpeg_reader_inner, &thread->args); +} + +struct thread_http_args { + struct wg *wg; + struct mpjpeg_stream *video; + struct mpjpeg_stream *depth; +}; +static +void *start_http_inner(void *args_anon) { + pthread_setname_np(pthread_self(), "http-listener"); + struct thread_http_args *args = args_anon; + thread_http_listener(args->wg, args->video, args->depth); + wg_sub(&wg, 1); + running = 0; + return NULL; +} +static +void start_http(struct wg *awg, + struct mpjpeg_stream *video, + struct mpjpeg_stream *depth) { + static struct thread_http_args args; + args.wg = awg; + args.video = video; + args.depth = depth; + + static pthread_t thread; + wg_add(&wg, 1); + pthread_create(&thread, NULL, start_http_inner, &args); +} + +static +void sigchld_handler(int sig UNUSED) +{ + int status; + while (waitpid(-1, &status, WNOHANG) > 0) {} +} + +static +void sigint_handler(int num UNUSED) +{ + running = 0; +} + +int main(int argc UNUSED, char **argv UNUSED) +{ + int kinect_video_fds[2]; + int kinect_depth_fds[2]; + int kinect_accel_fds[2]; + int mpjpeg_video_fds[2]; + int mpjpeg_depth_fds[2]; + struct mpjpeg_stream mpjpeg_video; + struct mpjpeg_stream mpjpeg_depth; + + struct sigaction act_chld; + sigemptyset (&act_chld.sa_mask); + act_chld.sa_flags = SA_RESTART; + act_chld.sa_handler = sigchld_handler; + if (sigaction(SIGCHLD, &act_chld, 0)) { + error(EXIT_FAILURE, errno, _("Could not set up SIGCHLD handler")); + } + + struct sigaction act_int; + sigemptyset (&act_int.sa_mask); + act_int.sa_flags = 0; + act_int.sa_handler = sigint_handler; + if (sigaction(SIGINT, &act_int, 0)) { + error(EXIT_FAILURE, errno, _("Could not set up SIGINT handler")); + } + + if (pipe(kinect_video_fds) == -1) error(1, errno, "pipe"); + if (pipe(kinect_depth_fds) == -1) error(1, errno, "pipe"); + if (pipe(kinect_accel_fds) == -1) error(1, errno, "pipe"); + if (pipe(mpjpeg_video_fds) == -1) error(1, errno, "pipe"); + if (pipe(mpjpeg_depth_fds) == -1) error(1, errno, "pipe"); + init_mpjpeg_stream(&mpjpeg_video); + init_mpjpeg_stream(&mpjpeg_depth); + + wg_init(&wg); + start_kinect(kinect_video_fds[1], + kinect_depth_fds[1], + kinect_accel_fds[1]); + start_mpjpeg_reader(&mpjpeg_video, mpjpeg_video_fds[0], "--ffserver"); + start_mpjpeg_reader(&mpjpeg_depth, mpjpeg_depth_fds[0], "--ffserver"); + start_http(&wg, &mpjpeg_video, &mpjpeg_depth); + /* never call exit() after we've started ffmpeg */ + start_ffmpeg(kinect_video_fds[0], mpjpeg_video_fds[1]); + start_ffmpeg(kinect_depth_fds[0], mpjpeg_depth_fds[1]); + wg_wait(&wg); + return 0; +} diff --git a/main.h b/main.h new file mode 100644 index 0000000..c4fd5dc --- /dev/null +++ b/main.h @@ -0,0 +1,39 @@ +#pragma once + +#include /* for sig_atomic_t */ +#include /* for memset(3) */ + +#define UNUSED __attribute__((__unused__)) +#define ZERO(x) memset(&(x), 0, sizeof(x)) + +#ifndef _ +#define _(str) str +#endif + +extern volatile sig_atomic_t running; + +struct frame { + ssize_t len; + size_t cap; + char *data; +}; + +struct mpjpeg_stream { + struct frame a; + struct frame b; + + struct frame *front; + struct frame *back; + pthread_mutex_t frontlock; +}; + +struct wg; + +void thread_kinect(int video_fd, int depth_fd, int accel_fd); +void thread_http_listen(int fd); +void thread_mpjpeg_reader(struct mpjpeg_stream *s, int fd, const char *boundary); +void thread_mpjpeg_writer(struct mpjpeg_stream *s, int fd, const char *boundary); +void thread_http_listener(struct wg *wg, struct mpjpeg_stream *video, struct mpjpeg_stream *depth); +void init_mpjpeg_stream(); + +void * xrealloc(void *ptr, size_t size); diff --git a/thread_http.c b/thread_http.c new file mode 100644 index 0000000..e410239 --- /dev/null +++ b/thread_http.c @@ -0,0 +1,124 @@ +#include +#include +#include /* for {get,free}addrinfo() */ +#include /* for EXIT_FAILURE */ +#include +#include +#include + +#include "main.h" +#include "wg.h" + +static +int tcp4_parse_listen(const char *port) { + int r; + struct addrinfo *addr = NULL; + struct addrinfo hints = { 0 }; + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + + if ((r = getaddrinfo(NULL, port, &hints, &addr)) != 0) + error(EXIT_FAILURE, r, _("Could not resolve TCP4 port %s"), port); + + int sock = socket(addr->ai_family, addr->ai_socktype, addr->ai_protocol); + if (sock < 0) + error(EXIT_FAILURE, errno, _("Could not create a TCP4 socket")); + + int yes = 1; + setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)); + + if (bind(sock, addr->ai_addr, addr->ai_addrlen) < 0) + error(EXIT_FAILURE, errno, _("Could not bind to TCP4 port %s"), port); + + if (listen(sock, 5) < 0) + error(EXIT_FAILURE, errno, _("Could not listen on TCP4 port %s"), port); + + freeaddrinfo(addr); + return sock; +} + +static struct wg *wg; +static struct mpjpeg_stream *video; +static struct mpjpeg_stream *depth; + +static +void thread_http_connection(int fd) { + FILE *stream = fdopen(fd, "r"); + char *line_buf = NULL; + size_t line_cap = 0; + getline(&line_buf, &line_cap, stream); + /* expect line to be "GET /kinect-(video|depth).mjpg HTTP/1.1" */ + if (strncmp(line_buf, "GET ", 4) != 0) { + dprintf(fd, + "HTTP/1.1 405 Method Not Allowed\r\n" + "Allow: GET\r\n" + "\r\n"); + close(fd); + return; + } + char *version = &line_buf[4]; + char *path = strsep(&version, " "); + if (strcmp(version, "HTTP/1.1\r\n") != 0 && strcmp(version, "HTTP/1.0\r\n")) { + close(fd); + return; + } + path = strdupa(path); + + /* run through the rest of the headers */ + while (strcmp(line_buf, "\r\n") != 0) + getline(&line_buf, &line_cap, stream); + + if (strcmp(path, "/kinect-video.mpjpg") == 0) { + dprintf(fd, + "HTTP/1.1 200 OK\r\n" + "Content-Type: multipart/x-mixed-replace;boundary=%s\r\n" + "\r\n", + "--boundary"); + thread_mpjpeg_writer(video, fd, "--boundary"); + } else if (strcmp(path, "/kinect-depth.mpjpg") == 0) { + dprintf(fd, + "HTTP/1.1 200 OK\r\n" + "Content-Type: multipart/x-mixed-replace;boundary=%s\r\n" + "\r\n", + "--boundary"); + thread_mpjpeg_writer(depth, fd, "--boundary"); + } else { + dprintf(fd, + "HTTP/1.1 404 Not Found\r\n" + "\r\n"); + } + close(fd); +} + +static +void *start_http_connection_inner(void *arg_anon) { + pthread_setname_np(pthread_self(), "http-connection"); + int fd = (int)(intptr_t)arg_anon; + thread_http_connection(fd); + wg_sub(wg, 1); + return NULL; +} + +static +void start_http_connection(int fd) { + wg_add(wg, 1); + pthread_t thread; + pthread_create(&thread, NULL, start_http_connection_inner, (void*)(intptr_t)fd); +} + +void thread_http_listener(struct wg *awg, + struct mpjpeg_stream *avideo, + struct mpjpeg_stream *adepth) { + wg = awg; + video = avideo; + depth = adepth; + int sock = tcp4_parse_listen("8090"); + + while (running) { + int conn = accept(sock, NULL, NULL); + if (conn < 0) + continue; + start_http_connection(conn); + } +} diff --git a/thread_kinect.c b/thread_kinect.c new file mode 100644 index 0000000..f3b5a78 --- /dev/null +++ b/thread_kinect.c @@ -0,0 +1,121 @@ +#include +#include +#include +#include +#include + +#include "main.h" + +FILE *depth_stream = NULL; +FILE *video_stream = NULL; +FILE *accel_stream = NULL; + +static +void dump_ffmpeg_24(FILE *stream, uint32_t timestamp UNUSED, void *data, + int data_size) +{ + fwrite(data, data_size, 1, stream); +} + +static +void dump_ffmpeg_pad16(FILE *stream, uint32_t timestamp UNUSED, void *data, + int data_size) +{ + unsigned int z = 0; + uint16_t* data_ptr = (uint16_t*)data; + uint16_t* end = data_ptr + data_size; + while (data_ptr < end) { + z = *data_ptr; + fwrite(((char*)(&z)), 3, 1, stream); + data_ptr += 2; + } +} + +static +void handle_accel(freenect_device *dev UNUSED, freenect_raw_tilt_state* data) +{ + 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); +} + +static +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); +} + +static +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); +} + +static +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 thread_kinect(int video_fd, int depth_fd, int accel_fd) { + int res = 0; + + freenect_context *ctx; + freenect_device *dev; + + pthread_setname_np(pthread_self(), "libusb"); + res = freenect_init(&ctx, 0); + pthread_setname_np(pthread_self(), "kinect"); + if (res) { + error(0, 0, "freenect_init: %s", libusb_strerror(res)); + goto end; + } + + freenect_select_subdevices(ctx, (freenect_device_flags)(FREENECT_DEVICE_CAMERA | FREENECT_DEVICE_MOTOR)); + if ((res = freenect_open_device(ctx, &dev, 0))) { + error(0, 0, "freenect_open_device: %s", libusb_strerror(res)); + goto end; + } + + 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); + + 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); + + depth_stream = fdopen(depth_fd, "w"); + video_stream = fdopen(video_fd, "w"); + accel_stream = fdopen(accel_fd, "w"); + + freenect_set_depth_callback(dev, handle_depth); + freenect_set_video_callback(dev, handle_video); + + while (running && freenect_process_events(ctx) >= 0) { + 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); + + end: + freenect_shutdown(ctx); + + if (depth_stream) fclose(depth_stream); + if (video_stream) fclose(video_stream); + if (accel_stream) fclose(accel_stream); +} diff --git a/thread_mpjpeg.c b/thread_mpjpeg.c new file mode 100644 index 0000000..112e1a2 --- /dev/null +++ b/thread_mpjpeg.c @@ -0,0 +1,90 @@ +/* Copyright 2016 Luke Shumaker */ + +#include +#include +#include +#include +#include + +#include "main.h" + +static +char *add_nl(const char *old) { + size_t len = strlen(old); + char *new = xrealloc(NULL, len+2); + strcpy(new, old); + new[len+0] = '\n'; + new[len+1] = '\0'; + return new; +} + +#define STARTS_WITH(str, prefix) strncmp(str, prefix, sizeof(prefix)-1) + +void thread_mpjpeg_reader(struct mpjpeg_stream *s, int fd, const char *boundary) { + FILE *stream = fdopen(fd, "r"); + boundary = add_nl(boundary); + + char *line_buf = NULL; + size_t line_cap = 0; + + while (1) { + s->back->len = -1; + /* read the frame header (MIME headers) */ + getline(&line_buf, &line_cap, stream); + if (strcmp(line_buf, boundary) != 0) + return; + ssize_t framelen = -1; + while (strcmp(line_buf, "\r\n") != 0) { + if (STARTS_WITH(line_buf, "Content-Type:")) { + s->back->len = atoi(&line_buf[sizeof("Content-Type:")-1]); + } + } + if (s->back->len < 0) + return; + + /* read the frame contents (JPEG) */ + if (s->back->cap < (size_t)s->back->len) + s->back->data = xrealloc(s->back->data, s->back->cap = s->back->len); + if (fread(line_buf, framelen, 1, stream) != 1) + return; + + /* swap the frames */ + pthread_mutex_lock(&s->frontlock); + struct frame *tmp = s->front; + s->front = s->back; + s->back = tmp; + pthread_mutex_unlock(&s->frontlock); + } + +} + +void thread_mpjpeg_writer(struct mpjpeg_stream *s, int fd, const char *boundary) { + struct frame myframe = { 0 }; + struct frame *lastframe = NULL; + while(1) { + /* get the most recent frame (copy front to myframe) */ + pthread_mutex_lock(&s->frontlock); + if (s->front == lastframe) { /* TODO: use a pthread_cond_t instead of a busy loop */ + pthread_mutex_unlock(&s->frontlock); + continue; + } + if (myframe.cap < (size_t)s->front->len) + myframe.data = xrealloc(myframe.data, myframe.cap = s->front->len); + memcpy(myframe.data, s->front->data, myframe.len = s->front->len); + lastframe = s->front; + pthread_mutex_unlock(&s->frontlock); + /* send the frame to the client */ + if (dprintf(fd, "%s\r\nContent-Type: image/jpeg\r\nContent-Length: %zd\r\n\r\n", boundary, myframe.len) < 0) + return; + if (write(fd, myframe.data, myframe.len) < myframe.len) + return; + } +} + +void init_mpjpeg_stream(struct mpjpeg_stream *s) { + ZERO(s->a); + ZERO(s->b); + s->front = &s->a; + s->back = &s->b; + pthread_mutex_init(&s->frontlock, NULL); +} diff --git a/wg.c b/wg.c new file mode 100644 index 0000000..3208aa1 --- /dev/null +++ b/wg.c @@ -0,0 +1,43 @@ +#include +#include +#include + +#include "wg.h" + +/* Thread management tools modeled on https://golang.org/pkg/sync/#WaitGroup */ + +/* pthread_cond_t is overly complicated. Just use a self-pipe. */ + +void wg_init(struct wg *wg) { + wg->count = 0; + pthread_mutex_init(&wg->lock, NULL); + int fds[2]; + if (pipe(fds) != 0) + error(1, errno, "pipe"); + wg->fd_wait = fds[0]; + wg->fd_signal = fds[1]; +} + +void wg_add(struct wg *wg, unsigned int n) { + pthread_mutex_lock(&wg->lock); + wg->count += n; + pthread_mutex_unlock(&wg->lock); +} + +void wg_sub(struct wg *wg, unsigned int n) { + pthread_mutex_lock(&wg->lock); + wg->count -= n; + if (wg->count == 0) + write(wg->fd_signal, " ", 1); + pthread_mutex_unlock(&wg->lock); +} + +void wg_wait(struct wg *wg) { + char b; + retry: + if (read(wg->fd_wait, &b, 1) == -1) { + if (errno == EINTR) + goto retry; + error(1, errno, "wg_wait"); + } +} diff --git a/wg.h b/wg.h new file mode 100644 index 0000000..777e8a3 --- /dev/null +++ b/wg.h @@ -0,0 +1,19 @@ +#pragma once + +#include + +/* Thread management tools modeled on https://golang.org/pkg/sync/#WaitGroup */ + +/* pthread_cond_t is overly complicated. Just use a self-pipe. */ + +struct wg { + int count; + pthread_mutex_t lock; + int fd_wait; + int fd_signal; +}; + +void wg_init(struct wg *); +void wg_add(struct wg *, unsigned int); +void wg_sub(struct wg *, unsigned int); +void wg_wait(struct wg*); -- cgit v1.2.3