ndirect.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

 /*********************************************************
 *
 * This source code is part of the Carnegie Mellon Robot
 * Navigation Toolkit (CARMEN)
 *
 * CARMEN Copyright (c) 2002 Michael Montemerlo, Nicholas
 * Roy, Sebastian Thrun, Dirk Haehnel, Cyrill Stachniss,
 * and Jared Glover
 *
 * CARMEN is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU General Public 
 * License as published by the Free Software Foundation; 
 * either version 2 of the License, or (at your option)
 * any later version.
 *
 * CARMEN is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied 
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the GNU General Public License for more 
 * details.
 *
 * You should have received a copy of the GNU General 
 * Public License along with CARMEN; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place, 
 * Suite 330, Boston, MA  02111-1307 USA
 *
 ********************************************************/

/*
 * Ndirect.c
 *
 * Implementation file for direct connection to robot, bypassing the need for
 * the Nserver program.
 *
 * Copyright 1991-1998, Nomadic Technologies, Inc.
 *
 */

char cvsid_host_client_Ndirect_c[] = "$Header: /cvsroot/carmen/carmen/src/base/scoutlib/Ndirect.c,v 1.2 2006/04/06 00:30:08 stachnis Exp $";

/* includes */

#include <stdlib.h>
#include <arpa/inet.h>
#include <fcntl.h>
#include <termios.h>
#include <signal.h>
#ifdef __USE_BSD
#undef __USE_BSD
#include <memory.h>
#define __USE_BSD
#else
#include <memory.h>
#endif
#include <errno.h>
#include <netdb.h>
#include <netinet/in.h>
#include <stdarg.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>          
#include <sys/socket.h>
#include <sys/time.h>           

#include "Nclient.h"


/* defines */

/* from command type.h  */

#define AC 1
#define SP 2
#define PR 3
#define PA 4
#define VM 5
#define MV 43
#define CT 6
#define GS 7
#define NAK 8
#define ST 9
#define LP 10
#define TK 11
#define DP 12
#define ZR 13
#define CONF_IR 14
#define CONF_SN 15
#define CONF_CP 16
#define CONF_LS 17
#define CONF_TM 18
#define GET_IR 19
#define GET_SN 20
#define GET_RC 21
#define GET_RV 22
#define GET_RA 23
#define GET_CP 24
#define GET_LS 25
#define SETUP_LS 26
#define GET_BP 27

#define CONF_SG 28
#define GET_SG 29

#define DA 30
#define WS 31

#define ADD_OBS 32
#define DELETE_OBS 33
#define MOVE_OBS 34

#define CONF_SER  35
#define PLACE_ROBOT 36

#define NUM_COMMANDS 36

#define SPECIAL 128

/* error types */
#define SERIAL_OPEN_ERROR 1
#define SERIAL_WRITE_ERROR 2
#define SERIAL_READ_ERROR 3
#define SERIAL_PKG_ERROR 4
#define SERIAL_TIMEOUT_ERROR 5

/* serial setting */
#define RE_XMIT 0            /* not re-transmit when failed */
#define NORMAL_TIMEOUT     1 /* 1 second */
#define CONNECT_TIMEOUT   10 /* 10 seconds */
#define SPECIAL_TIMEOUT   30 /* used for user define package */

/* 
 * Define the length of the user buffer (Maximal short).
 * Due to Protocol bytes, the effective length is 65526 
 */

#define USER_BUFFER_LENGTH	0xFFFF

/* from pos.h */

/* 
 * these macros enable the user to determine if the pos-attachment
 * is desired for a specific sensor, the argument is Smask[ SMASK_POS_DATA ]
 * to avoid overlap with Nclient.h suffix I for internal
 */

#define POS_INFRARED_PI(x)  ( ( (x) & POS_INFRARED ) ? 1 : 0 )
#define POS_SONAR_PI(x)     ( ( (x) & POS_SONAR    ) ? 1 : 0 )
#define POS_BUMPER_PI(x)    ( ( (x) & POS_BUMPER   ) ? 1 : 0 )
#define POS_LASER_PI(x)     ( ( (x) & POS_LASER    ) ? 1 : 0 )
#define POS_COMPASS_PI(x)   ( ( (x) & POS_COMPASS  ) ? 1 : 0 )


/* from datatype.h */

/* to build a long out ouf four bytes */

#define LONG_B(x,y,z,q) ((((x) << 24) & 0xFF000000) | \
                         (((y) << 16) & 0x00FF0000) | \
                         (((z) <<  8) & 0x0000FF00) | \
                         ( (q)        & 0x000000FF) )

/*
 * 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];        /* State reading */
int  Smask[NUM_MASK];         /* Sensor mask */
int  Laser[2*NUM_LASER+1];    /* Laser reading */


/* connect_type == 1 ---> serial */
/* connect_type == 2 ---> socket */
int    connect_type           = 1;
int    model;
char  *device;
int    conn_value;
int    DEFAULT_SERIAL_BAUD    = 38400;
int    DEFAULT_ROBOT_TCP_PORT = 65001;
double LASER_CALIBRATION[8]   = { -0.003470,  0.000008, 0.011963,  0.001830,
				  27.5535913, 0.000428, 0.031102, -0.444624 };
double LASER_OFFSET[2]        = { 0, 0 };

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

char   SERVER_MACHINE_NAME[80] = "";
int    SERV_TCP_PORT           = -1;
char   Host_name[255]          = "";

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

static int             Fd=-1;
static unsigned char   buf[BUFSIZE];
static unsigned char   *bufp, *bufe;
static int             errorp = 0;
static int             wait_time;
static int             usedSmask[NUM_MASK];       /* mask vector */
static int             Robot_id = -1;

/* although called special, it is the user buffer */
static unsigned char   *special_buffer;
static unsigned short  special_answer_size;

/* 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;

/* the laser mode */
static int laser_mode = 51;

/*******************
 *                 *
 * Functions (fwd) *
 *                 *
 *******************/

/* function declarations */

static long posLongExtract    ( unsigned char *inbuf );
static unsigned long posUnsignedLongExtract( unsigned char *inbuf );
static int posPackageProcess  ( unsigned char *inbuf, PosData *posData );
static int timePackageProcess ( unsigned char *inbuf, unsigned long *timeS );
static int voltPackageProcess ( unsigned char *inbuf,
			        unsigned char *voltCPU,
			        unsigned char *voltMotor);
static float voltConvert   ( unsigned char reading , float range );



/*******************************************************
 *                                                     *
 * Helper functions for manipulating bytes and numbers *
 *                                                     *
 *******************************************************/

static int low_half(unsigned char num)
{
  return (num % 16);
}

static int high_half(unsigned char num)
{
  return (num / 16);
}

/*
static unsigned char high_byte_signed(int n)
{
  int sign_num;
  
  if (n < 0) sign_num = 128; else sign_num = 0;
  return (sign_num + (abs(n) / 256));
}
*/
/*
static unsigned char low_byte_signed(int n)
{
  return (abs(n) % 256);
}
*/
/*
static unsigned char high_byte_unsigned(int n)
{
  return (n / 256);
}
*/
/*
static unsigned char low_byte_unsigned(int n)
{
  return (n % 256);
}
*/
static void signed_int_to_two_bytes(int n, unsigned char *byte_ptr)
{
  int sign_num;
  
  *byte_ptr = (unsigned char)(abs(n) % 256);
  byte_ptr++;
  if (n < 0) sign_num = 128; else sign_num = 0;
  *byte_ptr = (unsigned char)(sign_num + (abs(n) / 256));
}

static void unsigned_int_to_two_bytes(int n, unsigned char *byte_ptr)
{
  *byte_ptr = (unsigned char)(abs(n) % 256);
  byte_ptr++;
  *byte_ptr = (unsigned char)(abs(n) / 256);
}

static int two_bytes_to_signed_int(unsigned char low_byte,
				   unsigned char high_byte)
{
  int num;
  
  if (high_byte > 127)
    num = (-256 * (high_byte - 128)) - low_byte;
  else
    num = 256 * high_byte + low_byte;
  return (num);
}

static unsigned int two_bytes_to_unsigned_int(unsigned char low_byte,
					      unsigned char high_byte)
{
  unsigned int num;
  
  num = 256 * high_byte + low_byte;
  return (num);
}

static long combine_bumper_vector(unsigned char b1,
				  unsigned char b2,
				  unsigned char b3)
{
  long num;
  
  num = b1 + (b2 * 256) + (b3 * 65536);
  return (num);
}

static unsigned char bits_to_byte(char bt0, char bt1, char bt2, char bt3,
				  char bt4, char bt5, char bt6, char bt7)
{
  unsigned char  rbyte;
  
  rbyte = (unsigned char)(bt0 + (2*bt1) + (4*bt2) + (8*bt3) + (16*bt4) +
			  (32*bt5) + (64*bt6) + (128*bt7));
  return (rbyte);
}

static int serial_ready (int fd, int wait)
{
  fd_set          lfdvar;
  int             ready;
  struct timeval  timeout;
  
  FD_ZERO(&lfdvar);
  FD_SET(fd, &lfdvar);
  
  timeout.tv_sec = wait;
  timeout.tv_usec = 0;
  
  ready = select(fd+1, &lfdvar, NULL, NULL, &timeout);
  return(ready);
}

/* 
 * creates a package by adding initial byte, checksum, and end byte
 * and send the package to robot
 */
static int Write_Pkg(int fd, unsigned char *outbuf)
{
  int i, outbufLen, chk_sum; 
  int nleft, nwritten;
  
  /* create a package */
  outbuf[0] = 1;
  chk_sum = 0;
  outbufLen = outbuf[1]+1;
  for (i=0; i<=outbufLen; i++)
    chk_sum = chk_sum + outbuf[i];
  chk_sum = chk_sum % 256;
  outbufLen++;
  outbuf[outbufLen] = chk_sum;
  outbufLen++;
  outbuf[outbufLen] = 92;
  outbufLen++;

  /* send package */
  for (nleft = outbufLen; nleft > 0; ) {
    nwritten = write(fd, outbuf, nleft);
    if (nwritten <= 0) {
      errorp = SERIAL_WRITE_ERROR;
      return(FALSE);
    }
    nleft = nleft - nwritten;
    outbuf = outbuf + nwritten;
  }
  return(TRUE);
}


/* 
 * read response from the robot 
 */
static unsigned char buf_fill(int fd, int conn_type)
{
  int n;
  
  if (conn_type == 1) 
  {
    n = read(fd, buf, BUFSIZE);
    if (n < 0) {
      printf("error reading serial port\n");
      errorp = SERIAL_READ_ERROR;
      return(0);
    }
    else {
      if (n == 0) {
	printf("serial port read timeout error\n");
	errorp = SERIAL_TIMEOUT_ERROR;
	return(0);
      }
    }
  } else {
    if (serial_ready(Fd, 100)) {
      n = read(Fd, buf, BUFSIZE);
      if (n < 0) {
        printf("TCP/IP communication broken.\n");
        errorp = SERIAL_READ_ERROR;
        return 0;
      } else {
        if (n == 0) {
          printf("TCP/IP select/read error\n");
          errorp = SERIAL_READ_ERROR;
          return(0);
        }
      }
    } else {
      printf("TCP/IP read timeout error.\n");
      errorp = SERIAL_TIMEOUT_ERROR;
      return 0;
    }
  }
  
  bufp = &buf[1];
  bufe = buf + n;
  return(buf[0]);
}

static unsigned char GETC(int fd, int conn_type)
{
  if (bufp<bufe)
    return(*bufp++);
  return(buf_fill(fd, conn_type));
}

/*
 * getting package from robot and 
 * check for package error: initial byte = 1, end byte = 92, check sum
 */
static int Read_Pkg(int fd, int conn_type, unsigned char *inbuf)
{