nclient.c

机器人仿真软件

/* This file was obtained from the nomadics respository on
   Sourceforge. I understand that it was released under the GPL by the
   copyright holders. Anyone with more information about the licensing
   or authorship of this code, please contact Richard Vaughan
   ([e-mail]).

   $Header: /cvsroot/playerstage/code/player/server/drivers/mixed/nomad/Nclient.c,v 1.4 2005/05/22 06:02:06 thjc Exp $
*/

/*
 * Nclient.c
 *
 * Implementation file for connection to robot and/or simulator via Nserver.
 *
 * Copyright 1991-98, Nomadic Technologies, Inc.
 *
 */

/* -- fixed a bug where initializing with an invalid robot id hung because
 *    Nclient tried to initialize sensors anyway -- rak, 16jun99
 */

/* defines */

/* generic message types */
#define MAX_MESSAGE             100

#ifndef FALSE
#define FALSE			0
#endif
#ifndef TRUE
#define TRUE			1
#endif
#define NEW_SOCKET       	2

#define MAX_VERTICES            10    /* maximum number of verices in a poly */
#define MAX_HOST_NAME_LENGTH	100
#define IS_SERVER_ALIVE_MSG	0     /* message types */
#define IS_SERVER_ALIVE_ACK	1

/* request message types */
#define AC_MSG                  10
#define SP_MSG                  11
#define VM_MSG                  12
#define PR_MSG                  13
#define MV_MSG                  43
#define PA_MSG                  14
#define GS_MSG                  15
#define CT_MSG                  16
#define ST_MSG                  17
#define LP_MSG                  18
#define TK_MSG                  19
#define ZR_MSG                  20
#define DP_MSG                  21
#define CONF_IR_MSG             22
#define CONF_SN_MSG             23
#define CONF_CP_MSG             24
#define CONF_LS_MSG             25
#define CONF_TM_MSG             26
#define GET_IR_MSG              27
#define GET_SN_MSG              28
#define GET_RC_MSG              29
#define GET_RV_MSG              30
#define GET_RA_MSG              31
#define GET_CP_MSG              32
#define GET_LS_MSG              33
#define SETUP_LS_MSG            34
#define GET_BP_MSG              35
#define CONF_SG_MSG             36
#define GET_SG_MSG              37
#define GET_RPX_MSG             75
#define RPX_MSG                 76
#define DA_MSG                  38
#define WS_MSG                  39

#define RMOVE_MSG               40
#define RPLACE_MSG              41
#define PREDICTSENSOR_MSG       42

#define ADDOBS_MSG  	        50
#define DELETEOBS_MSG           51
#define MOVEOBS_MSG             52
#define NEWWORLD_MSG            53

#define DRAWROBOT_MSG           60
#define DRAWLINE_MSG            61
#define DRAWARC_MSG             62

#define QUIT_MSG                70
#define REALROBOT_MSG           71
#define SEND_MESSAGE_MSG        72
#define RECEIVE_MESSAGE_MSG     73
#define SIMULATEDROBOT_MSG      74

#define REFRESHALL_MSG         150
#define REFRESHALLTRACES_MSG   151
#define REFRESHACTTRACE_MSG    152
#define REFRESHENCTRACE_MSG    153
#define REFRESHALLSENSORS_MSG  154
#define REFRESHBPSENSOR_MSG    155
#define REFRESHIRSENSOR_MSG    156
#define REFRESHSNSENSOR_MSG    157
#define REFRESHLSSENSOR_MSG    158
#define REFRESHGRAPHICS_MSG    159

/* reply message types */

#define STATE_MSG               80
#define MOVED_MSG               81
#define REPLY_MSG               82 
#define INFRARED_MSG            83
#define SONAR_MSG               84
#define LASER_MSG               85
#define COMPASS_MSG             86
#define BUMPER_MSG              87
#define CONFIGURATION_MSG       88
#define VELOCITY_MSG            89
#define ACCELERATION_MSG        90
#define ERROR_MSG               91

#define CREATE_ROBOT_MSG        101
#define CONNECT_ROBOT_MSG       102
#define DISCONNECT_MSG          103

#define GET_CONF_MSG            200

#define SPECIAL_MSG             300

#define MCHECK_MSG              400
#define INTERSECT_MSG           401

#define MAX_VERT     10
#define NUM_STATE    45
#define NUM_MASK     44 
#define NUM_LASER    482 
#define MAX_MESSAGE  100
#define MAX_USER_BUF 0xFFFF

#define SERIAL_ERROR 11
#define IPC_ERROR    111

/* constants */
#define MAX_VERTICES     10
#define NUM_STATE        45
#define NUM_MASK         44 
#define NUM_LASER        482 
#define MAX_MESSAGE      100


/* includes */

#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <memory.h>
#include <errno.h>
#include <sys/types.h>          /* for "bind" system calls */
#include <sys/time.h>           /* for "select" system call */
#include <sys/socket.h>         /* for socket calls */
#ifdef _AIX32
#include <sys/select.h>
#endif
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <fcntl.h>
#include <signal.h>
#include "Nclient.h"

#define DEBUG
#undef DEBUG

/*
 * The voltages have different ranges to account for the fact that the
 * CPU measurement is taken after lossage on the slip-ring.
 */

#define RANGE_CPU_VOLTAGE        12.0
#define RANGE_MOTOR_VOLTAGE      12.85

/********************
 *                  *
 * Type definitions *
 *                  *
 ********************/

/*
 * PosDataAll is a struct that contains the Pos information for
 * all sensors. It is used to pass/store the Pos info within the 
 * server. It contains the laser, although the laser is not
 * used in the dual ported ram.
 */

typedef struct _PosDataAll
{
  PosData infrared [INFRAREDS];
  PosData sonar    [SONARS   ];
  PosData bumper;
  PosData laser;
  PosData compass;

} PosDataAll;


/********************
 *                  *
 * Global Variables *
 *                  *
 ********************/

long   State[NUM_STATE];
int    Smask[NUM_MASK];
int    Laser[2*NUM_LASER+1];
char   SERVER_MACHINE_NAME[80] = "";
int    SERV_TCP_PORT           = 7019;

/* dummy variables to stay compatible with Ndirect.c */

char   ROBOT_MACHINE_NAME[80] = "";
int    CONN_TYPE       = -1;
char   NOMAD_SERIAL_PORT[256] = "";
int    NOMAD_SERIAL_BAUD     = -1;
int    ROBOT_TCP_PORT  = -1;

double LASER_CALIBRATION[8];
double LASER_OFFSET[2];

/*******************
 *                 *
 * Local Variables *
 *                 *
 *******************/

static int   own_tcp_port = 0;
static int   dest_socket = 0;
static int   usedSmask[NUM_MASK];       /* mask vector */
static char  Host_name[255] = ""; /* name of the host computer where the 
				     client is running */
static int   laser_mode = 51;
static int   connectedp = 0;

static struct request_struct the_request;
static struct reply_struct the_reply;

/* this is where all the incoming posData is stored */
static PosDataAll posDataAll;
static unsigned long posDataTime;

/* for the voltages of motor/CPU as raw data */
static unsigned char voltageCPU;
static unsigned char voltageMotor;

/* function declaration */

//int gethostname(char *name, int len);
static int posDataProcess  (long *buffer, int current, PosData *posData);
static int timeDataProcess (long *buffer, int current, TimeData *theTime );
static int voltDataProcess (long *buffer, int current, 
			    unsigned char *voltCPU, unsigned char *voltMotor);
static float voltConvert   ( unsigned char reading , float range );
char *convertAddr ( char *name, char *addr );

/*************************
 *                       *
 * Some helper functions *
 *                       *
 *************************/

/*
 * open_socket_to_send_data - this function opens the socket and initializes
 *                            it properly for sending data through.  This
 *                            function returns "-1" if it cannot establish
 *                            a connection.
 */
char *convertAddr ( char *name, char *addr )
{
  int addrInt[10];

  sscanf(name, "%d.%d.%d.%d", 
	 &(addrInt[0]), &(addrInt[1]), &(addrInt[2]), &(addrInt[3]));
  addr[0] = addrInt[0];
  addr[1] = addrInt[1];
  addr[2] = addrInt[2];
  addr[3] = addrInt[3];
  return ( addr );
}


static int open_socket_to_send_data(int tcp_port_num)
{
  int s;
  struct hostent *hp;
  struct sockaddr_in serv_addr;
  char addr[10];
  
  if (!(strcmp(Host_name, "")))
  {
    if (!(strcmp(SERVER_MACHINE_NAME,"")))
      gethostname(Host_name, 100);
    else 
      strcpy(Host_name, SERVER_MACHINE_NAME);
  }
  if ( ((hp = gethostbyname(Host_name)) == NULL) &&
       ((hp = gethostbyaddr(convertAddr(Host_name,addr),4,AF_INET)) == NULL ) )
  {
    printf("host %s not valid\n", Host_name); 
#ifdef DEBUG
    perror("ERROR: in open_socket_to_send_data, gethostbyname failed");
#endif
    strcpy(Host_name, "");
    return(0);
  }
  /*
  bzero((char *) &serv_addr, sizeof(serv_addr));
  bcopy(hp->h_addr, (char *) &(serv_addr.sin_addr), hp->h_length);
  */
  memset ( (char *) &serv_addr, 0, sizeof(serv_addr) );
  memcpy ( (char *) &(serv_addr.sin_addr), hp->h_addr, hp->h_length );

  serv_addr.sin_family = AF_INET;            /* address family */
  serv_addr.sin_port = htons(tcp_port_num);  /* internet TCP port number */
  
  if ((s = socket(AF_INET, SOCK_STREAM, 0)) < 0)
  {
#ifdef DEBUG
    printf("ERROR: in open_socket_to_send_data, socket failed.\n");
#endif
    return(0);
  }
  
  if (connect(s, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
  {
#ifdef DEBUG
    printf("ERROR: in open_socket_to_send_data, connect failed.\n");
#endif
    return(-1);
  }
  
  return(s);
}

/*
 * readn - this function reads "n" bytes from a socket before returning.
 */
static int readn(register int fd, register char *ptr, register int nbytes)
{
  int nleft, nread;
  
  for (nleft = nbytes; nleft > 0; )
  {
    nread = read(fd, ptr, nleft);
    if (nread <= 0)
    {
      if (nread < 0 && (errno == EINTR || errno == EAGAIN))
      {
	nread = 0;
      } else {
	return(nread);
      }
    }
    
    nleft -= nread;
    ptr += nread;
  }
  return(nbytes - nleft);
}

/*
 * read_reply_from_socket - read reply from server
 */
static int read_reply_from_socket(int sock, struct reply_struct *this_reply)
{
  unsigned short i;
  short rep_size, rep_type;
  long n, mesg_size;
  /* 
     mesg size has to be a long, because bytes are carried as longs, thus the
     maximum size is 0xFFFF*4, which needs a long to be represented;
     this waists lots of bandwidth. I do not change it now, because we will
     move soon to a decent socket communications protocol 
     */
  n = readn(sock, (char *) &rep_type, sizeof(short));
  n = readn(sock, (char *) &rep_size, sizeof(short));
  if (n <= 0)
  {
    if (n == 0)
    {
#ifdef DEBUG
      printf("ERROR: read 0 byte\n");
#endif
      return(-1);
    }
    else
    {
#ifdef DEBUG
      printf("ERROR: in read_reply_from_socket.\n");
#endif
      return(-1);
    }
  }
  
  this_reply->type = ntohs(rep_type);
  this_reply->size = ntohs(rep_size);
  mesg_size = this_reply->size*sizeof(long);
  n = readn(sock, (char *) this_reply->mesg, mesg_size);
  
  if (n < mesg_size)
  {
#ifdef DEBUG
    printf("ERROR: in read_request_from_socket, readn 2 didn't.\n");
#endif
    return(-1);
  }
  else
  {
    for (i=0; i<this_reply->size; i++) {
      this_reply->mesg[i] = ntohl(this_reply->mesg[i]);
    }
  }
  return(this_reply->type);
}

/*
 * writen - this function writes "n" bytes down a socket before returning.
 */
static int writen(register int fd, register char *ptr, register int nbytes)
{
  int nleft, nwritten;
  
  for (nleft = nbytes; nleft > 0; )
  {
    nwritten = write(fd, ptr, nleft);
    if (nwritten <= 0)
    {
      if (nwritten < 0 && (errno == EINTR || errno == EAGAIN))
      {
	nwritten = 0;
      } else {
	return(nwritten);
      }
    }
    
    nleft -= nwritten;
    ptr += nwritten;
  }
  
  return(nbytes - nleft);
}

/* 
 * write_request_to_socket - sends the reply to the client who requested
 *                           service
 */
static int write_request_to_socket(int sock, struct request_struct *this_request)
{
  int i, request_size;
  
  /* changing to network data format before sending */
  this_request->type = htons(this_request->type);
  for (i=0; i<this_request->size; i++)
  {
    this_request->mesg[i] = htonl(this_request->mesg[i]);
  }
  request_size = 2*sizeof(short) + this_request->size*sizeof(long);
  this_request->size = htons(this_request->size);
  
  return(writen(sock, (char *)this_request, request_size));
}

/* 
 * ipc_comm - sends a request to the server and gets a reply from it
 */
static int ipc_comm(struct request_struct *this_request,
		    struct reply_struct *this_reply)
{
  int read_result;
  
  if (!connectedp)
  {
    printf("Not connected to any robot\n");
    return(FALSE);
  }
  
  if (!dest_socket)
  {
    if (own_tcp_port)
      dest_socket = open_socket_to_send_data(own_tcp_port);
    else
      dest_socket = open_socket_to_send_data(SERV_TCP_PORT);
    
    if (dest_socket <= -1)
      return(FALSE);		
  }
  write_request_to_socket(dest_socket, this_request);
  
  if (this_reply)
  {
    read_result = read_reply_from_socket(dest_socket, this_reply);
    return(read_result != -1);
  }
  else
  {
    close(dest_socket); /* this_reply is set to NULL to indicate disconnect; close the socket */
    return(TRUE);
  }