//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;
}
int main(int argc, char *argv[])
{
srand(time(0));
while (1) {
// Pick a random tilt and a random LED state
freenect_led_options led = (freenect_led_options) (rand() % 6); // explicit cast
int tilt = (rand() % 30)-15;
freenect_raw_tilt_state *state = 0;
double dx, dy, dz;
// Set the LEDs to one of the possible states
if (freenect_sync_set_led(led, 0)) no_kinect_quit();
// Set the tilt angle (in degrees)
if (freenect_sync_set_tilt_degs(tilt, 0)) no_kinect_quit();
// Get the raw accelerometer values and tilt data
if (freenect_sync_get_tilt_state(&state, 0)) no_kinect_quit();
// Get the processed accelerometer values (calibrated to gravity)
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("led[%d] tilt[%d] accel[%lf,%lf,%lf]\n", led, tilt, dx,dy,dz);
sleep(3);
}
}
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");
}
}
//Waiting for a client to connect
void *waiting_for_client(void *arg)
{
int childlen;
struct sockaddr_in childaddr;
childlen = sizeof(childaddr);
while (!die)
{
printf("### Wait client\n");
#ifdef _WIN32 //Wait for connection (Winsock)
data_client_socket = accept(data_socket, NULL, NULL);
if (data_client_socket == INVALID_SOCKET) {
if(!die) printf("Error on accept() for data, exit data thread. %d\n", WSAGetLastError());
closesocket(data_socket);
WSACleanup();
break;
}
#else //Wait for connection (UNIX)
data_child = accept(s_data, (struct sockaddr *)&childaddr, (unsigned int *)&childlen);
if ( data_child < 0 )
{
if(!die) fprintf(stderr, "Error on accept() for data, exit data thread.\n");
break;
}
#endif
//Handle client connection and create connection thread to communicate with client
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");
}
//Then we start listening again to allow re-connection to server
}
return NULL;
}
// 3.5 (fun_kled_g) - freenect_sync_set_led_green_
void freenect_sync_set_led_green_cv(int index)
{
int led_green=0;
led_green = freenect_sync_set_led(LED_GREEN, index);
//return 0;
}
// 3.4 (fun_kled_r) - freenect_sync_set_led_red_cv
void freenect_sync_set_led_red_cv(int index)
{
int led_red=0;
led_red = freenect_sync_set_led(LED_RED, index);
//return 0;
}
// 3.8 (fun_kled_off)- freenect_sync_set_led_off_cv
void freenect_sync_set_led_off_cv(int index)
{
int led_green=0;
led_green = freenect_sync_set_led(LED_OFF, index);
//return 0;
}
// 3.7 (fun_kled_y) - freenect_sync_set_led_yellow_cv
void freenect_sync_set_led_yellow_cv(int index)
{
int led_green=0;
led_green = freenect_sync_set_led(LED_YELLOW, index);
//return 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;
}
///functions
int main(int argc, char* argv[])
{
//input parameters
if(argc != 3){
printf("usage: %s <first ip> <second ip>\n", argv[0]);
return EXIT_FAILURE;
}
//set Kinect angles to 0° & set LED colour
if(freenect_sync_set_tilt_degs(0, 0)){
printf("Could not tilt device 0.\n");
return EXIT_FAILURE;
}
if(freenect_sync_set_led(LED_GREEN, 0)){
printf("Could not change LED of device 0.\n");
return EXIT_FAILURE;
}
//set UDP socket
struct sockaddr_in si_other, si_other2;
int s, i, slen=sizeof(si_other);
char buf[BUFLEN];
if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1){
fprintf(stderr, "socket() failed\n");
return 1;
}
//first IP
memset((char *) &si_other, 0, sizeof(si_other));
si_other.sin_family = AF_INET;
si_other.sin_port = htons(PORT);
if (inet_aton(argv[1], &si_other.sin_addr)==0) {
fprintf(stderr, "inet_aton() failed\n");
return 1;
}
//second IP
memset((char *) &si_other2, 0, sizeof(si_other2));
si_other2.sin_family = AF_INET;
si_other2.sin_port = htons(PORT);
if (inet_aton(argv[2], &si_other2.sin_addr)==0) {
fprintf(stderr, "inet_aton() failed\n");
return 1;
}
//set cameras
TDepthCamera mainCam;
createPrimaryCamera(&mainCam, 0);
//get calibration values acquired by calibration program.
FILE* pFile = NULL;
pFile = fopen("calibrationValuesOne.cal", "r");
if(pFile == NULL){
puts("Could not get calibration data.");
}else{
fread(&minZ, sizeof(int), 1, pFile);
fread(&maxZ, sizeof(int), 1, pFile);
}
fclose(pFile);
TVecList mainList;
unsigned int timestamp;
contLoop = 1;
//show current calibration values.
printf("Current calibration values:\nCeiling: %d, Floor: %d\n", maxZ, minZ);
puts("\n\nAre those values correct? [Y/N]");
char tmpChar = getchar();
if(tmpChar == 'N' || tmpChar == 'n'){
contLoop = 0;
puts("\nUse calibration program to correct the values.");
}
fflush(stdin);
//start thread
pthread_t thread;
int rc;
long t = 0;
rc = pthread_create(&thread, NULL, readAsync, (void *)t);
if (rc){
printf("ERROR; return code from pthread_create() is %d\n", rc);
exit(-1);
}
//main loop
while(contLoop){
//acquire data for main Kinect & process data
if(updateCamera(&mainCam, ×tamp)){
printf("Could not update feed for device 0.");
return EXIT_FAILURE;
}
if(detectDrone(mainCam.data, &mainList, &vec3DDistance)){
printf("Could not process data for for device 0.");
return EXIT_FAILURE;
}
//display list
system("clear");
puts("Press Enter to exit.\n\n---------------\nLIST:");
displayVecList(&mainList);
//send position to the given IP address
TVec4D* maxVect = maxPointList(&mainList);
if(maxVect != NULL){
writePacket(buf, 'k', maxVect->x, maxVect->y, maxVect->z);
if (sendto(s, buf, BUFLEN, 0, &si_other, slen)==-1){
fprintf(stderr, "sendto() failed\n");
return 1;
}
if (sendto(s, buf, BUFLEN, 0, &si_other2, slen)==-1){
fprintf(stderr, "sendto() failed\n");
return 1;
}
}
}
//close socket
close(s);
//free all data
freeCamera(&mainCam);
//stop kinects
freenect_sync_stop();
//stop pthread
pthread_exit(NULL);
return EXIT_SUCCESS;
}
//Connection protocol handler
void *connection_handler(void *arg) {
int n, value;
//send_policy_file(data_child);
while(client_connected) {
char buff[8*10]; //Command buffer
#ifdef WIN32 //Listen for data (Winsock)
n = recv(data_client_socket, (char*)buff, sizeof(buff), 0);
#else //Listen for data (UNIX)
n = read(data_child, buff, sizeof(buff));
#endif
//If command length is multiple of 6
if(n > 0 && n % 6 == 0){
//Get the number of commands received
int max = n / 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 RGB
sendVideo();
break;
case 2: //Mirror depth
_depth_mirrored = value;
break;
case 3: //Mirror video
_video_mirrored = value;
break;
case 4: //Min depth
_min_depth = value;
break;
case 5: //Max depth
_max_depth = 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)
printf("got bad command (%d)\n", n);
client_connected = 0;
}
}
printf("Disconecting client...\n");
//waiting for client led status
freenect_sync_set_led((freenect_led_options) 4, 0);
return NULL;
}
