network.cpp

用于机器人自动低分辨路的地图测绘程序。用于机器人控制测绘。分为远端控制端和本地控制端。控制电机为标准舵机。

/*
    Robot Interface
    (C) 2006 Jason Hunt
    [e-mail]
    
    File: network.cpp
*/


#include <windows.h>
#include <stdio.h>

#include "network.h"
#include "main.h"
#include "display.h"
#include "types.h"
#include "camera.h"
#include "robot.h"
#include "serial.h"
#include "time.h"

//extern unsigned int max_compressed;
//extern unsigned char *compressed_buffer;

unsigned int camera_port = 32459;


//extern bool remote_mode;

SOCKET listen_socket = INVALID_SOCKET;  // Server will listen on this socket
SOCKET client_socket = INVALID_SOCKET;  // This will be the client's socket

bool client_connected = false;

unsigned int client_updates = 0;

//HANDLE socket_sem = CreateSemaphore(NULL,1,2, NULL);

CRITICAL_SECTION network_send_lock;



double last_client_time;

/* Start socket system and setup to listen for a client */
void network_start( HWND hwnd )
{
    WSADATA wsda;
    WSAStartup(MAKEWORD(1, 1),&wsda);    
    
    InitializeCriticalSection(&network_send_lock);
    
    // Create listen socket
    listen_socket = socket(AF_INET, SOCK_STREAM, 0);

    if (listen_socket == INVALID_SOCKET)
    {
        add_line("Socket error");
        return;
    }

    u_long arg = 1; // Arg to allow non blocking

    // Make accept non bocking
    if (ioctlsocket(listen_socket, FIONBIO, &arg) != 0)
    {
        add_line("Unable to listen");
        return;
    }

    // Setup parameters
    SOCKADDR_IN server;
    server.sin_addr.s_addr = INADDR_ANY;
    server.sin_port = htons(camera_port);
    server.sin_family = AF_INET;

    // Bind socket 
    if (bind(listen_socket, (LPSOCKADDR) &server, sizeof(struct sockaddr)) == SOCKET_ERROR)
    {
        add_line("Unable to bind");
        return;
    }

    // Defeate Nagle Algorithm 
    BOOL val = TRUE;

    if (setsockopt(listen_socket, IPPROTO_TCP, TCP_NODELAY, (char*)&val, sizeof(BOOL)) == SOCKET_ERROR) 
    {
        add_line("Unable to set TCP_NODELAY");
        return;
    }

    // Setup listen
    if (listen(listen_socket, 1) == SOCKET_ERROR)
    {
        add_line("Unable to listen");
        return;
    }
  
    // Setup message system
    if (WSAAsyncSelect(listen_socket, hwnd, WM_NETEVENT, FD_READ | FD_WRITE |
                                           FD_ACCEPT | FD_CLOSE | FD_CONNECT))
    {
        add_line("WSAAsyncSelect fail");
        return;          
    }    
    
    add_line("Network Online");
}
/* End of start network */


/* Stops the network */
void network_stop( void )
{
    closesocket(client_socket);
    closesocket(listen_socket);
    remote_mode = false;

    WSACleanup();
}
/* End of shutdown_network */   
    

/* Called when a client is trying to connect */
void network_accept(void)
{
    int client_size = sizeof(sockaddr_in);

    // A client is already connected
    if (client_socket != INVALID_SOCKET) 
    {
        sockaddr_in tmp_client; 

        // Accept to a temp socket
        SOCKET tmp_socket = accept(listen_socket,(struct sockaddr*)&tmp_client, &client_size);

        // Close immediately
        closesocket(tmp_socket);

        add_line("Another client attempted to connect");
        return; 
    }

    sockaddr_in client; // Client socket information

    // Accept the client
    client_socket = accept(listen_socket,(struct sockaddr*)&client, &client_size);

    if (client_socket == INVALID_SOCKET)
    {
        add_line("Accept error.");
        return;
    }

    // Display client address
    char out[BUFFER_SIZE];
    sprintf(out, "Connection from: %s", inet_ntoa(client.sin_addr));
    add_line(out);
    
    client_connected = true;
    
    remote_mode = true;
        
    last_client_time = get_time();
}
/* End of network accept */

void netword_send_telemetry( void )
{
    unsigned char buff[50];

    robot.get_telemetry(buff+1);
     
    buff[0] =  0x90;
    network_send(buff, 21);           
}

/* Send the currently captured image to the client */
void network_send_image( void )
{
   // SetEvent(request_image);        
    
   camera.request_image();
    
 //   camera_frames++;
   // add_line("send");
//    camera.get_image();   
}
/* End of send_client_image */


/* Send Data to client */
int network_send(byte *data, unsigned int size)
{
     bool fail = false;

    EnterCriticalSection(&network_send_lock);
 
    // Send the compressed data
    if (send(client_socket, (char *)data, size, 0) == SOCKET_ERROR)     
    {
        fail = true;
        client_connected = false;
        
        robot.reset_control();
    } 

    LeaveCriticalSection(&network_send_lock);

    return(fail);   
}
/* End of network_send */

            
/* Called when client data is ready */
void network_read(void)
{
    byte buffer[BUFFER_SIZE];
  
    int n_bytes = recv(client_socket, (char*)buffer, BUFFER_SIZE, 0); // Get the data
   
    if (n_bytes <= 0) return; // Error
    
    unsigned int i = 0;
    
    
/*    char b[300];
    sprintf(b, "Bytes: %d", n_bytes);
if (n_bytes > 1)
    add_line(b);*/
    
    while(i < n_bytes)
    {
        if (buffer[i] == ROBOT_MOVEMENT)
        {           
            last_client_time = get_time();
            robot.set_control(buffer+i+1);
            i += SERIAL_SEND_SIZE;                      
   netword_send_telemetry();
        }else

//        if (buffer[i] == ROBOT_SENSOR_DATA) // Sensor data request
//        {        
//           // robot_send_sensor_data();
//            i++;                            // Skip to next                                       
//        } else    


        if (buffer[i] == CAMERA_IMAGE)      // Image request from client
        {        
           network_send_image();            // CLient is requesting an image
           netword_send_telemetry();
            i++;                            // Skip to next               
        } else       
            i++;                            // Ignore, skip to attemp resync
    }    
}
/* End of read_client_data */



/* Check for network remote updates */
void network_check( void )
{
    if (client_connected)
    {
        double cur_time = get_time();
    
        if (cur_time - last_client_time > 1)
        {
            add_line("Client data Timeout");

            robot.reset_control();
        }
    }
}
/* End of network_check */


/* Deal with network messages */
void network_message(WPARAM wParam, LPARAM lParam)
{
    switch (WSAGETSELECTEVENT(lParam))
    {
        case FD_ACCEPT:
            network_accept();
            break;

        case FD_CONNECT:
            break;

        case FD_CLOSE:
            add_line("Client disconnect");
            robot.reset_control();
            closesocket(client_socket);
            client_socket = INVALID_SOCKET;
            client_connected = false;      
            remote_mode = false;      
            break;

        case FD_READ:
            // make sure the socket requesting reading is our client
            if (wParam == client_socket)
                network_read();
            break;
    }
}
/* End of network_message */