split into separate executables

1 files changed, 165 insertions, 0 deletions

diff --git a/freenect-server--kinect.c b/freenect-server--kinect.c
new file mode 100644
index 0000000..1c68fe3
--- /dev/null
+++ b/freenect-server--kinect.c
@@ -0,0 +1,165 @@
+#include <errno.h>
+#include <error.h>
+#include <stdio.h>
+#include <stdlib.h> /* atexit */
+#include <unistd.h> /* dup2 */
+
+#include <libfreenect/libfreenect.h>
+#include <libusb-1.0/libusb.h>
+
+#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) {
+		uint32_t z = *data;
+		fwrite(((char*)(&z)), 3, 1, stream);
+		data = &data[1];
+	}
+}
+
+void handle_accel(freenect_device *dev UNUSED, freenect_raw_tilt_state* data)
+{
+	printf("handle accel\n");
+	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);
+}
+
+void handle_depth(freenect_device *dev UNUSED, void *depth, uint32_t timestamp)
+{
+	printf("handle depth\n");
+	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)
+{
+	printf("handle video\n");
+	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");
+				break;
+			case 'd':
+				i++;
+				depth_stream = fopen(argv[i], "w");
+				break;
+			case 'a':
+				i++;
+				accel_stream = fopen(argv[i], "w");
+				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);
+	}
+
+	if (accel_stream) {
+	}
+
+	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);
+}