summaryrefslogtreecommitdiff
path: root/src/freenect-server.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/freenect-server.c')
-rw-r--r--src/freenect-server.c130
1 files changed, 77 insertions, 53 deletions
diff --git a/src/freenect-server.c b/src/freenect-server.c
index a36701c..286455b 100644
--- a/src/freenect-server.c
+++ b/src/freenect-server.c
@@ -24,10 +24,13 @@
#include <errno.h>
#include <error.h>
#include <inttypes.h>
+#include <stdbool.h>
#include <stdio.h>
-#include <stdlib.h> /* atexit */
+#include <stdlib.h> /* atexit, EXIT_FAILURE */
#include <unistd.h> /* dup2 */
+#include <systemd/sd-daemon.h>
+
#include <libfreenect/libfreenect.h>
#include <libusb-1.0/libusb.h>
@@ -35,10 +38,12 @@
#include "util.h"
-FILE *depth_stream = NULL;
-FILE *video_stream = NULL;
-FILE *accel_stream = NULL;
-freenect_context *ctx;
+void stop(int sig);
+#define threaderror(stat, errnum, ...) do { \
+ error(0, errnum, __VA_ARGS__); \
+ exitcode = stat; \
+ stop(0); \
+ } while(0)
struct mpjpg_encoder {
struct jpeg_error_mgr jpeg_encoder_err;
@@ -46,10 +51,17 @@ struct mpjpg_encoder {
unsigned char *jpeg_buf;
unsigned long jpeg_len;
};
-
+
+int exitcode = 0;
+bool running = true;
+FILE *depth_stream = NULL;
+FILE *video_stream = NULL;
+FILE *accel_stream = NULL;
+freenect_context *ctx = NULL;
+freenect_device *dev = NULL;
struct mpjpg_encoder depth_encoder;
struct mpjpg_encoder video_encoder;
-
+
void mpjpg_init(struct mpjpg_encoder *e) {
e->jpeg_encoder.err = jpeg_std_error(&e->jpeg_encoder_err);
jpeg_create_compress(&e->jpeg_encoder);
@@ -63,7 +75,12 @@ void mpjpg_init(struct mpjpg_encoder *e) {
jpeg_mem_dest(&e->jpeg_encoder, &e->jpeg_buf, &e->jpeg_len);
}
-void write_imageframe(FILE *stream, struct mpjpg_encoder *e, JSAMPLE *data) {
+void mpjpg_destroy(struct mpjpg_encoder *e) {
+ jpeg_destroy_compress(&e->jpeg_encoder);
+ free(e->jpeg_buf);
+}
+
+void write_imageframe(const char *name, FILE *stream, struct mpjpg_encoder *e, JSAMPLE *data) {
JSAMPLE *row_pointers[480];
for (size_t i = 0; i < 480; i++)
row_pointers[i] = &data[i*640*3];
@@ -79,14 +96,14 @@ void write_imageframe(FILE *stream, struct mpjpg_encoder *e, JSAMPLE *data) {
"Content-length: %lu\r\n"
"\r\n",
e->jpeg_len) < 1)
- error(EXIT_FAILURE, ferror(stream), "writing header");
+ threaderror(EXIT_FAILURE, ferror(stream), "%s: writing header", name);
if (fwrite(e->jpeg_buf, e->jpeg_len, 1, stream) < 1)
- error(EXIT_FAILURE, ferror(stream), "writing body");
+ threaderror(EXIT_FAILURE, ferror(stream), "%s: writing body", name);
if (fwrite("\r\n", 2, 1, stream) < 1)
- error(EXIT_FAILURE, ferror(stream), "writing footer");
+ threaderror(EXIT_FAILURE, ferror(stream), "%s: writing footer", name);
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);
@@ -116,14 +133,16 @@ void handle_accel(freenect_device *dev UNUSED, freenect_raw_tilt_state* data) {
" \"motor\": \"%s\" }\n",
x, y, z, angle, motor);
if (len < 0)
- error(EXIT_FAILURE, errno, "asprintf");
- fprintf(accel_stream,
+ threaderror(EXIT_FAILURE, errno, "accel.mjson: asprintf");
+ if (fprintf(accel_stream,
"--ffserver\r\n"
"Content-type: application/json\r\n"
"Content-length: %d\r\n"
"\r\n"
"%s\r\n",
- len, json);
+ len, json) < 0)
+ threaderror(EXIT_FAILURE, ferror(accel_stream), "accel.mjson: write");
+ free(json);
fflush(accel_stream);
}
@@ -131,46 +150,28 @@ 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);
+ threaderror(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_encoder, depth_rgb);
+ write_imageframe("depth.mjpg", depth_stream, &depth_encoder, 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);
+ threaderror(EXIT_FAILURE, 0, "handle_video: expected 640*480*3 byte frame, but got %"PRId32, size);
/* write the image */
- write_imageframe(video_stream, &video_encoder, 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);
+ write_imageframe("video.mjpg", video_stream, &video_encoder, rgb);
}
-
-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 */
+
+void stop(int sig) {
+ if (sig != 0)
+ log("Caught %d", sig);
+ sd_notify(0, "STOPPING=1");
+ running = false;
}
FILE *xfopen(const char *path, const char *mode) {
@@ -194,10 +195,13 @@ 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);
}
+void cleanup(void);
+
int main(int argc, char *argv[]) {
int res = 0;
- freenect_device *dev;
+ mpjpg_init(&video_encoder);
+ mpjpg_init(&depth_encoder);
atexit(cleanup);
for (int i = 1; i < argc; i++) {
@@ -247,9 +251,6 @@ int main(int argc, char *argv[]) {
return EXIT_FAILURE;
}
- mpjpg_init(&depth_encoder);
- mpjpg_init(&video_encoder);
-
res = freenect_init(&ctx, 0);
if (res) {
error(EXIT_FAILURE, 0, "freenect_init: %s", libusb_strerror(res));
@@ -267,20 +268,26 @@ int main(int argc, char *argv[]) {
}
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) {
+ signal(SIGPIPE, SIG_IGN);
+ signal(SIGTERM, stop);
+ signal(SIGQUIT, stop);
+ signal(SIGINT, stop);
+ while (running) {
+ if (freenect_process_events(ctx) < 0) {
+ stop(0);
+ continue;
+ }
if (accel_stream) {
freenect_raw_tilt_state* state;
freenect_update_tilt_state(dev);
@@ -288,8 +295,25 @@ int main(int argc, char *argv[]) {
handle_accel(dev, state);
}
}
+}
- freenect_stop_depth(dev);
- freenect_stop_video(dev);
- freenect_close_device(dev);
+void cleanup() {
+ if (dev) {
+ freenect_stop_depth(dev);
+ freenect_stop_video(dev);
+ freenect_close_device(dev);
+ }
+ if (ctx)
+ freenect_shutdown(ctx);
+ if (video_stream)
+ fclose(video_stream);
+ if (depth_stream)
+ fclose(depth_stream);
+ if (accel_stream)
+ fclose(accel_stream);
+ mpjpg_destroy(&video_encoder);
+ mpjpg_destroy(&depth_encoder);
+ fflush(stderr);
+ if (sd_booted() > 0) sleep(5); /* work around systemd bug dropping log messages */
+ _exit(exitcode);
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-18 21:11:19 +0000