as3-server.c

Go to the documentation of this file.

/*
 * This file is part of the OpenKinect Project. http://www.openkinect.org
 *
 * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
 * for details.
 *
 * 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 <stdio.h>
#include <stdlib.h>

#ifndef _WIN32
        #include <unistd.h>
        #include <arpa/inet.h>
#else
        #include <winsock2.h>
        #include <ws2tcpip.h>
        #pragma comment(lib, "Ws2_32.lib")
        #include <stdint.h>
        #include <windows.h>
        unsigned sleep(unsigned seconds)
        {
          Sleep(seconds*1000);  // The Windows Sleep operates on milliseconds
          return(0);
        }
#endif

#include <string.h>
#include "libfreenect_sync.h"
#include "libfreenect.h"
#include <pthread.h>
#include <math.h>

#include "freenect_network.h"
#include "as3_jpeg.h"
#include "libfreenect_sync.h"

int g_argc;
char **g_argv;

char *_current_version = "v0.9c";
pthread_t connection_thread;

#define AS3_BITMAPDATA_LEN 640 * 480 * 4

uint8_t buf_depth[AS3_BITMAPDATA_LEN];
uint8_t buf_rgb[AS3_BITMAPDATA_LEN];
void *buf_depth_temp;
void *buf_rgb_temp;

int die = 0;
int psent = 0;

int _video_mirrored = 0,
        _depth_mirrored = 0,
        _min_depth = 600,
        _max_depth = 800,
        _video_compression = 80,
        _depth_compression = 20;
        
int client_connected = 0;

#ifdef _WIN32
        unsigned sleep(unsigned seconds)
        {
          Sleep(seconds*1000);  // The Windows Sleep operates on milliseconds
          return(0);
        }
#endif

void send_policy_file(int child){
        /*if(psent == 0){
                char * str = "<?xml version='1.0'?><!DOCTYPE cross-domain-policy SYSTEM '/xml/dtds/cross-domain-policy.dtd'><cross-domain-policy><site-control permitted-cross-domain-policies='all'/><allow-access-from domain='*' to-ports='*'/></cross-domain-policy>\n";
                #ifdef WIN32
                int n = send(data_client_socket, (char*)str, 235, 0);
                #else
                int n = write(child,str , 235);
                #endif
                
                if ( n < 0 || n != 235)
                {
                        fprintf(stderr, "Error on write() for policy (%d instead of %d)\n",n, 235);
                }
                //psent = 1;
        }*/
}

//send depth ARGB to client
void sendDepth(){
        int n;
        uint32_t ts, x, y, i, j;
        freenect_sync_get_depth(&buf_depth_temp, &ts, 0, FREENECT_DEPTH_11BIT);
        uint16_t *depth = (uint16_t*) buf_depth_temp;
        freenect_frame_mode depth_mode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT);
        //uint16_t *tmp_depth = (uint16_t*) malloc(FREENECT_DEPTH_11BIT_SIZE);
        uint16_t *tmp_depth = (uint16_t*) malloc(depth_mode.bytes);
        if(_depth_mirrored){    //MIRROR DEPTH DATA
                for(x = 0; x < depth_mode.width; x++){
                        for(y = 0; y < depth_mode.height; y++){
                                i = x + (y  * depth_mode.width);
                                j = (depth_mode.width - x - 1) + (y  * depth_mode.width);
                                tmp_depth[i] = depth[j];
                        }
                }
                depth = tmp_depth;
        }
        
    for (i=0; i< depth_mode.width * depth_mode.height; i++) {
                if(_depth_compression != 0) {
                        buf_depth[3 * i + 0] = 0x00;
                        buf_depth[3 * i + 1] = 0x00;
                        buf_depth[3 * i + 2] = 0x00;
                } else {
                        buf_depth[4 * i + 0] = 0x00;
                        buf_depth[4 * i + 1] = 0x00;
                        buf_depth[4 * i + 2] = 0x00;
                        buf_depth[4 * i + 3] = 0xFF;
                }
                if(depth[i] < _max_depth && depth[i] > _min_depth){
                        unsigned char l =  0xFF - ((depth[i] - _min_depth) & 0xFF);
                        if(_depth_compression != 0) {
                                buf_depth[3 * i + 0] = l;
                                buf_depth[3 * i + 1] = l;
                                buf_depth[3 * i + 2] = l;
                        } else {
                                buf_depth[4 * i + 0] = l;
                                buf_depth[4 * i + 1] = l;
                                buf_depth[4 * i + 2] = l;
                                buf_depth[4 * i + 3] = 0xFF;
                        }
                }
        }
        if(_depth_compression != 0) {
                unsigned char *compressed_buff = (unsigned char *)malloc(AS3_BITMAPDATA_LEN);
                unsigned long len = 0;
                RGB_2_JPEG(buf_depth, &compressed_buff, &len, _depth_compression);
                n = freenect_network_sendMessage(0, 0, compressed_buff, (int)len);
                free(compressed_buff);
        } else {
                n = freenect_network_sendMessage(0, 0, buf_depth, AS3_BITMAPDATA_LEN);
        }
        if (n < 0)
        {
                printf("Error sending depth\n");
                client_connected = 0;
        }
        free(tmp_depth);
}

//send depth ARGB to client
void sendRawDepth(){
        uint32_t ts, x, y, i, j;
        freenect_sync_get_depth(&buf_depth_temp, &ts, 0, FREENECT_DEPTH_11BIT);
        uint16_t *depth = (uint16_t*) buf_depth_temp;
        freenect_frame_mode depth_mode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT);
        uint16_t *tmp_depth = (uint16_t*) malloc(depth_mode.bytes);
        unsigned char *char_depth = (unsigned char *) malloc(depth_mode.bytes);
        
        if(_depth_mirrored){    //MIRROR DEPTH DATA
                for(x = 0; x < depth_mode.width; x++){
                        for(y = 0; y < depth_mode.height; y++){
                                i = x + (y  * depth_mode.width);
                                j = (depth_mode.width - x - 1) + (y  * depth_mode.width);
                                tmp_depth[i] = depth[j];
                        }
                }
                depth = tmp_depth;
        }
        
        for (i=0; i<640 * 480; i++) {
                memcpy(char_depth + (i*2), &depth[i], 2);
        }
        
        int n = freenect_network_sendMessage(0, 1, char_depth, depth_mode.bytes);
        if (n < 0)
        {
                printf("Error sending raw depth\n");
                client_connected = 0;
        }
        free(char_depth);
        free(tmp_depth);
}

//send video ARGB to client
void sendVideo(){
        int n;
        uint32_t ts,x, y, i, j;
        freenect_sync_get_video(&buf_rgb_temp, &ts, 0, FREENECT_VIDEO_RGB);
        uint8_t *rgb = (uint8_t*)buf_rgb_temp;
        freenect_frame_mode video_mode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB);

        //MIRROR RGB DATA AND ADD ALPHA
        for(x = 0; x < video_mode.width; x++){
                for(y = 0; y < video_mode.height; y++){
                        i = x + (y  * video_mode.width);
                        if(!_video_mirrored)
                                j = i;
                        else
                                j = (video_mode.width - x - 1) + (y  * video_mode.width);
                        if(_video_compression != 0) {
                                buf_rgb[3 * i + 2] = rgb[3 * j + 2];
                                buf_rgb[3 * i + 1] = rgb[3 * j + 1];
                                buf_rgb[3 * i + 0] = rgb[3 * j + 0];
                        } else {
                                buf_rgb[4 * i + 0] = rgb[3 * j + 2];
                                buf_rgb[4 * i + 1] = rgb[3 * j + 1];
                                buf_rgb[4 * i + 2] = rgb[3 * j + 0];
                                buf_rgb[4 * i + 3] = 0x00;      
                        }
                }
        }
        if(_video_compression != 0) {
                unsigned char *compressed_buff = (unsigned char *)malloc(AS3_BITMAPDATA_LEN);
                unsigned long len = 0;
                RGB_2_JPEG(buf_rgb, &compressed_buff, &len, _video_compression);
                n = freenect_network_sendMessage(0, 2, compressed_buff, (int)len);
                free(compressed_buff);
        } else {
                n = freenect_network_sendMessage(0, 2, buf_rgb, AS3_BITMAPDATA_LEN);
        }
        if ( n < 0)
        {
                printf("Error sending Video\n");
                client_connected = 0;
        }
}

//send accelerometer data to client
void sendAccelerometers() {
        int16_t ax,ay,az;
        double dx,dy,dz;
        unsigned char buffer_send[3*2+3*8];
        freenect_raw_tilt_state *state;
        freenect_sync_get_tilt_state(&state, 0);
        ax = state->accelerometer_x;
        ay = state->accelerometer_y;
        az = state->accelerometer_z;
        freenect_get_mks_accel(state, &dx, &dy, &dz);
        memcpy(&buffer_send,&ax, sizeof(int16_t));
        memcpy(&buffer_send[2],&ay, sizeof(int16_t));
        memcpy(&buffer_send[4],&az, sizeof(int16_t));
        memcpy(&buffer_send[6],&dx, sizeof(double));
        memcpy(&buffer_send[14],&dy, sizeof(double));
        memcpy(&buffer_send[22],&dz, sizeof(double));
        int n = freenect_network_sendMessage(1, 2, buffer_send, 3*2+3*8);
        if ( n < 0)
        {
                printf("Error sending Accelerometers\n");
                client_connected = 0;
        }
}

//Connection protocol handler
void *connection_handler(void *arg) {
        int value;
        int len = 8*10;
        char *buff = (char*)malloc(len); //Command buffer
        //send_policy_file(data_child);
        while(client_connected) {
                freenect_network_read(buff, &len);
                //If command length is multiple of 6
                if(len > 0 && len % 6 == 0){
                        //Get the number of commands received
                        int max = len / 6;
                        int i;
                        //For each command received
                        for(i = 0; i < max; i++){
                                memcpy(&value, &buff[2 + (i*6)], sizeof(int));
                                value = ntohl(value);
                                //The BIG switch (Communication protocol)
                                switch(buff[0 + (i*6)]){
                                        case 0: //CAMERA
                                                switch(buff[1 + (i*6)]){
                                                        case 0: //GET DEPTH
                                                                sendDepth();
                                                        break;
                                                        case 1: //GET RAW DEPTH
                                                                sendRawDepth();
                                                        break;
                                                        case 2: //GET RGB
                                                                sendVideo();
                                                        break;
                                                        case 3: //Mirror depth
                                                                _depth_mirrored = value;
                                                        break;
                                                        case 4: //Mirror video
                                                                _video_mirrored = value;
                                                        break;
                                                        case 5: //Min depth
                                                                _min_depth = value;
                                                        break;
                                                        case 6: //Max depth
                                                                _max_depth = value;
                                                        break;
                                                        case 7: //Depth compression
                                                                _depth_compression = value;
                                                        break;
                                                        case 8: //Video compression
                                                                _video_compression = value;
                                                        break;
                                                }
                                        break;
                                        case 1: //MOTOR
                                                switch(buff[1 + (i*6)]){
                                                        case 0: //MOVE
                                                                freenect_sync_set_tilt_degs(value, 0);
                                                        break;
                                                        case 1: //LED COLOR
                                                                freenect_sync_set_led((freenect_led_options) value, 0);
                                                        break;
                                                        case 2: //Accelerometer
                                                                sendAccelerometers();
                                                        break;
                                                }
                                        break;
                                }
                        }
                } else { //Command was not multiple of 6 (we received an invalid command)
                        if(!die) printf("got bad command (%d)\n", len   );
                        client_connected = 0;
                }
        }
        if(!die) {
                printf("Disconecting client...\n");
                freenect_network_wait();
                //waiting for client led status
                freenect_sync_set_led((freenect_led_options) 4, 0);
        }
        return NULL;
}

void server_connected(){
        printf("###### Got client\n");
        //got client led status
        freenect_sync_set_led((freenect_led_options) 1, 0);
        client_connected = 1;
        if (pthread_create(&connection_thread, NULL, &connection_handler, NULL)) {
                fprintf(stderr, "Error on pthread_create() for SERVER\n");
        }
}

//Called when C-c or C-z is pressed
#ifdef _WIN32
void clean_exit(int){
        die = 1;
}
#else
void clean_exit(){
        printf("clean exit called\n");
        die = 1;
        freenect_network_close();
}
#endif
//Main: we start here
int main(int argc, char **argv)
{
        printf("as3kinect server %s\n", _current_version);
        //waiting for client led status
        freenect_sync_set_led((freenect_led_options) 4, 0);
        //Listening to C-c
        if (signal (SIGINT, clean_exit) == SIG_IGN)
        signal (SIGINT, SIG_IGN);
        //Listening to C-z
        #ifndef _WIN32
        if (signal (SIGTSTP, clean_exit) == SIG_IGN)
        signal (SIGTSTP, SIG_IGN);
        #endif
        //Alloc memory for video and depth frames
        buf_depth_temp = malloc(FREENECT_DEPTH_11BIT);
        buf_rgb_temp = malloc(FREENECT_VIDEO_RGB);
        
        //Initialize network socket
        if ( freenect_network_init(server_connected) < 0 )
                die = 1;
        
        //Sleep main process until exit
        while(!die)
                sleep(2);

        //making a clean exit
        free(buf_depth_temp);
        free(buf_rgb_temp);
        //device off led status
        freenect_sync_set_led((freenect_led_options) 0, 0);
        freenect_sync_stop();
        printf("\n[as3-server] cleanup done!\n");
        return 0;
}