network.cpp

来自「用于机器人自动低分辨路的地图测绘程序。用于机器人控制测绘。分为远端控制端和本地控」· C++ 代码 · 共 396 行

/*
    Robot Interface Remote Client
    (C) 2006 Jason Hunt
    [e-mail]

    file: network.h
*/

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

#include "types.h"
#include "main.h"
#include "network.h"
#include "display.h"
#include "image.h"
#include "decompress.h"
#include "network.h"
#include "robot.h"
#include "target.h"

extern HWND main_win; 
extern HWND system_hwnd;

//extern unsigned char *compressed_image;


extern unsigned int frames; 

extern bool debug;



extern bool camera_active;


char server_hostname[200] = "127.0.0.1";    // Default hostname

unsigned int server_port = 32459;

SOCKET server_socket = INVALID_SOCKET;  // This will be the client's socket
bool network_comm_ok = false;
bool network_connecting = false;


byte recv_buffer[BUFFER_SIZE];

unsigned int bytes_remaining = 0;
unsigned int image_index = 0;
bool receiving_image = false;



/**********************************
*** Network interface functions ***
**********************************/

/* Check network status, reconnect if failed */
void network_check( void )
{
    if (network_comm_ok) return;

   // if (++reconnect_wait < 3) return; // only try to reconnect every three seconds

    network_connect();              // Reconnect
}
/* End of check_network */

     
/* Start socket system and setup to listen for a client */
void network_start( void )
{
    // Fire up winsock
    WSADATA wsda;
    WSAStartup(MAKEWORD(1, 1),&wsda);
   
    network_connect();
}
/* End of start network */


/* Stops the network */
void network_stop( void )
{
    closesocket(server_socket);        
    
    WSACleanup();
}
/* End of shutdown_network */   


/*****************************************
*** End of Network interface functions ***
*****************************************/




/* Connect to the camrera server */
void network_connect( void )
{
    // Network is not usable now
    network_comm_ok = false;

    // Do not try to connect if already connecting
    if (network_connecting) return;

    // Close existing
    closesocket(server_socket);
        
    // Create the socket
    if ((server_socket = socket(AF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET)
    {
        add_line("Network: Unable to create socket");
        return;   
    }

    u_long arg = 1; // Arg to allow non blocking
      
    // Make accept non bocking
    if (ioctlsocket(server_socket, FIONBIO, &arg) != 0)
    {
        add_line("Camera: Unable to listen");
        return;
    }
    
    BOOL bOptVal = TRUE;
    int bOptLen = sizeof(BOOL);

    // Defeat Nagel
    if (setsockopt(server_socket, IPPROTO_TCP, TCP_NODELAY, (char*)&bOptVal, bOptLen) == SOCKET_ERROR) return;

    // Get hostname
    LPHOSTENT hostent = gethostbyname(server_hostname);

    // Check
    if (!hostent) 
    {
        add_line("Network: Unable to resolve hostname");
        return;        
    }

    // Setup address information 
    SOCKADDR_IN server;

    server.sin_addr=*((LPIN_ADDR)*hostent->h_addr_list);
    server.sin_port=htons(server_port);
    server.sin_family=AF_INET;

    // Connect to the server (This is non blocking)
    connect(server_socket,(LPSOCKADDR)&server, sizeof(struct sockaddr));
                
    // Setup message system
    WSAAsyncSelect(server_socket, system_hwnd, WM_NETEVENT, FD_READ | FD_WRITE |
                                                   FD_ACCEPT | FD_CLOSE | FD_CONNECT);}
/* End of conenct network */
    

/* Send a single command byte to the server */
void send_command(  char c )
{
    if (!network_comm_ok) return;
        
    if (send(server_socket, &c, 1, 0) == SOCKET_ERROR)    
    { 
        network_comm_ok = false;
      //  add_line("Unable to send data");          
    }
}
/* End of send_command */


/* Request an image from the server */
void request_image( void )
{
//add_line("image request");

    if (receiving_image) return;


    send_command(CAMERA_IMAGE);
}
/* End of request image */


/* Send data to the client */
int network_send(unsigned char *data, unsigned int size)
{
    if (send(server_socket, (char*)data,size, 0) == SOCKET_ERROR) 
    { 
        network_comm_ok = false;
        return(1);
    }
        return(0);
}
/* End of network_send */


/* Send data to the client */
int network_send(unsigned char data)
{
    if (send(server_socket, (char *)&data, 1, 0) == SOCKET_ERROR)
    {
        network_comm_ok = false;
        return(1);  
    }
        return(0);
}
/* End of network_send */


/* Called when client data is ready */
void read_data(void)
{
    int n_bytes = recv(server_socket, (char*)recv_buffer, BUFFER_SIZE, 0); // Get the data

    if (n_bytes < 1) return; // Error

    if (debug)
    {
        FILE *f = fopen("r.txt", "at");
        fprintf(f, "bytes: %d\n", n_bytes);
        
        if (n_bytes < 10)
        {
            for (int t = 0; t < n_bytes; t++)
                    fprintf(f, "%x ", recv_buffer[t]);
            fprintf(f, "\n");
        }
        fclose(f);
    }

    unsigned int i = 0;

    while(i < n_bytes)
    {
        if (receiving_image)
        {
            // Data block is not greater than the bytes left
            // we subtract the i because those bytes were already used
        
            unsigned int packet_bytes = n_bytes-i;
            
            if (packet_bytes > bytes_remaining)
                packet_bytes = bytes_remaining;

            memcpy((char*)compressed_image + image_index, recv_buffer + i, packet_bytes);
            
            image_index += packet_bytes;

            i += packet_bytes;
         
            bytes_remaining -= packet_bytes;
              
                
            if (bytes_remaining == 0)
            {
                // Decompress the image to the display

                request_image(); // Request next image    

                decompress_image((char *)compressed_image, image_index, image_buffer, image_x_size, image_y_size);

                if (targeting)
                    process_frame();

                receiving_image = false;



                frames++;



                    InvalidateRect(main_win, NULL, 0);
                
                //display();
                
              /*  static int image = 0;
                image++;
                
                char b[400];
                sprintf(b, "image: %d", image);
                add_line(b);*/
                
                
                Sleep(10);
                request_image(); // Request next image                
            }

        }
        else

    //    camera_image_start(recv_buffer + i + i, n_bytes);
        
        if (recv_buffer[i] == CAMERA_IMAGE) // image from server
        {                        
            unsigned int x_size = (recv_buffer[i+1] << 8) + recv_buffer[i+2];
            unsigned int y_size = (recv_buffer[i+3] << 8) + recv_buffer[i+4];
        
            unsigned int data_size = (recv_buffer[i+5] << 24) + (recv_buffer[i+6] << 16) +
                                     (recv_buffer[i+7] << 8)  + recv_buffer[i+8];
//   add_line("image data");
   
            // Sanity check for image size
            if (x_size > 1024 || y_size > 768)
            {
                add_line("Network: Image size invalid");
                network_comm_ok = false;
                return;
            }            
   
            if (x_size > 0 && y_size > 0)
            {
                camera_active = true;
                // resize buff if changed
                if (x_size != image_x_size || y_size != image_y_size)
                {
                    image_x_size = x_size;
                    image_y_size = y_size;
             
                    image_start();                      // Resize image buffer        
                }
        
                receiving_image = true;                 // Now receving image
                bytes_remaining = data_size;            // Set target byte count
                image_index = 0;
            }

            i += 9; // Skip to next data
        } else
        if (recv_buffer[i] ==  ROBOT_SENSOR_DATA) // sensor data
        {
            // Send data to robot module to deal with
            // adjust index based on the amount of data used
            i += robot_sensor_data(recv_buffer + i + 1, n_bytes - i) + 1;
        }   
        else
            i++;
    }
}
/* End of read_client_data */


void network_connect_message( LPARAM lParam )
{
  char b[400]; 
    if ( HIWORD(lParam) == 0)
    {
        sprintf(b, "Camera: Connected to %s:%d", server_hostname, server_port);
        add_line(b);

        network_comm_ok = true;     // Now connected    

        robot_reset();               
                
        request_image();            // Request first image
                        
    } else
    {
        sprintf(b, "Network: Unable to connect to %s:%d", server_hostname, server_port);

        add_line(b);

        network_comm_ok = false; 
    }
            
    network_connecting = false;   
}

/* Deal with network messages */
void network_message(WPARAM wParam, LPARAM lParam)
{
    switch (WSAGETSELECTEVENT(lParam))
    {
        case FD_CONNECT:
            network_connect_message(lParam);      
            return;

        case FD_CLOSE:
            add_line("Network: Disconnected");
            camera_active = false;
            closesocket(server_socket);
            server_socket = INVALID_SOCKET;
            network_comm_ok = false;
            return;

        case FD_READ:
            read_data();
            return;
    }
}
/* End of network_message */