/********************************************************* * * 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){