OrocosComponentLibrary: SickLMS200.cpp Source File

00001 #include "SickLMS200.h"
00002 #include <errno.h>
00003 #include <sys/types.h>
00004 #include <sys/stat.h>
00005 #include <fcntl.h>
00006 #include <termios.h>
00007 //#include <stdio.h>
00008 //#include <sys/time.h>
00009 //#include <time.h>
00010 #include <signal.h>
00011 //#include <getopt.h>
00012 #include <unistd.h>
00013 #include <string.h>
00014 #include <stdlib.h>
00015 #include <stdio.h>
00016 #include <rtt/Logger.hpp>
00017 
00018 #define FALSE 0
00019 #define TRUE 1
00020 #define MAXRETRY 100
00021 #define MAXNDATA 802
00022 #define STX 0x02   /*every PC->LMS packet is started by STX*/
00023 #define ACKSTX 0x06 /*every PC->LMS packet is started by ACKSTX*/
00024 typedef unsigned char uchar;
00025 
00026 using namespace RTT;
00027 
00028 
00029 namespace SickDriver {
00030 /*the cmd and ack packets for the 5 different range/resolution modes*/
00031 const uchar PCLMS_RES1[]={0x02,0x00,0x05,0x00,0x3b,0x64,0x00,0x64,0x00,0x1d,0x0f};
00032 const uchar PCLMS_RES2[]={0x02,0x00,0x05,0x00,0x3b,0x64,0x00,0x32,0x00,0xb1,0x59};
00033 const uchar PCLMS_RES3[]={0x02,0x00,0x05,0x00,0x3b,0x64,0x00,0x19,0x00,0xe7,0x72};
00034 const uchar PCLMS_RES4[]={0x02,0x00,0x05,0x00,0x3b,0xb4,0x00,0x64,0x00,0x97,0x49};
00035 const uchar PCLMS_RES5[]={0x02,0x00,0x05,0x00,0x3b,0xb4,0x00,0x32,0x00,0x3b,0x1f};
00036 const uchar LMSPC_RES1_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0x64,0x00,0x64,0x00,0x10,0x4f,0xbd};
00037 const uchar LMSPC_RES2_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0x64,0x00,0x32,0x00,0x10,0x17,0x10};
00038 const uchar LMSPC_RES3_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0x64,0x00,0x19,0x00,0x10,0xbb,0x46};
00039 const uchar LMSPC_RES4_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0xb4,0x00,0x64,0x00,0x10,0x5b,0x30};
00040 const uchar LMSPC_RES5_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0xb4,0x00,0x32,0x00,0x10,0x03,0x9d};
00041 
00042 /*the cmd and ack packets different measurement unit modes*/
00043 const uchar PCLMS_SETMODE[]={0x02,0x00,0x0a,0x00,0x20,0x00,0x53,0x49,0x43,0x4b,0x5f,0x4c,0x4d,0x53,0xbe,0xc5};
00044 const uchar PCLMS_CM[]={0x02,0x00,0x21,0x00,0x77,0x00,0x00,0x00,0x00,0x00,0x0d,0x00,0x00,0x00,\
00045             0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,\
00046             0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x20,0xcb};
00047 const uchar PCLMS_MM[]={0x02,0x00,0x21,0x00,0x77,0x00,0x00,0x00,0x00,0x00,0x0d,0x01,0x00,0x00,\
00048             0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,\
00049             0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x34,0xc7};
00050 const uchar LMSPC_CM_ACK[]={0x06,0x02,0x80,0x25,0x00,0xf7,0x00,0x00,0x00,0x46,0x00,0x00,0x0d,0x00,\
00051                 0x00,0x00,0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,\
00052                 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0xcb,0x10};
00053 const uchar LMSPC_MM_ACK[]={0x06,0x02,0x80,0x25,0x00,0xf7,0x00,0x00,0x00,0x46,0x00,0x00,0x0d,0x01,\
00054                 0x00,0x00,0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,\
00055                 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0xc7,0x10};
00056 
00057 /*the cmd packets for setting transfer speedn and  controlling the start and stop of measurement*/
00058 const uchar PCLMS_START[]={0x02,0x00,0x02,0x00,0x20,0x24,0x34,0x08};
00059 const uchar PCLMS_STOP[]={0x02,0x00,0x02,0x00,0x20,0x25,0x35,0x08};
00060 const uchar PCLMS_STATUS[]={0x02,0x00,0x01,0x00,0x31,0x15,0x12};
00061 const uchar PCLMS_B9600[] ={0x02,0x00,0x02,0x00,0x20,0x42,0x52,0x08};
00062 const uchar PCLMS_B19200[]={0x02,0x00,0x02,0x00,0x20,0x41,0x51,0x08};
00063 const uchar PCLMS_B38400[]={0x02,0x00,0x02,0x00,0x20,0x40,0x50,0x08};
00064 /*the ack packet for the above*/
00065 const uchar LMSPC_CMD_ACK[]={0x06,0x02,0x80,0x03,0x00,0xa0,0x00,0x10,0x16,0x0a};
00066 
00067 
00068 
00069 
00070 
00071 bool SickLMS200::msgcmp(int len1, const uchar *s1, int len2, const uchar *s2)
00072 {
00073   int i;
00074   if(len1!=len2) return FALSE;
00075   for(i=0; i<len1; i++)
00076     if(s1[i]!=s2[i]) return FALSE;
00077   return TRUE;
00078 }
00079 
00080 
00081 void SickLMS200::wtLMSmsg(int fd, int len, const uchar *msg)
00082 {
00083   write(fd,(const void*) msg,len);
00084 }
00085 
00086 int SickLMS200::rdLMSmsg(int fd, int len, const uchar *buf)
00087 {
00088   int sumRead=0,nRead=0,toRead=len,n;
00089   while(toRead>0){
00090     n=toRead>255 ? 255:toRead;
00091     toRead-=n;
00092     nRead=read(fd,(void *)(buf+sumRead),n);
00093     sumRead+=nRead;
00094     if(nRead!=n) break;
00095   }
00096   return nRead;
00097 }
00098 
00099 uchar SickLMS200::rdLMSbyte(int fd)
00100 {
00101   uchar buf;
00102   read(fd,&buf,1);
00103   return buf;
00104 }
00105 
00106 
00107 
00108 /*return true if the ACK packet is as expected*/
00109 bool SickLMS200::chkAck(int fd, int ackmsglen, const uchar *ackmsg)
00110 {
00111   int i,buflen;
00112   uchar buf[MAXNDATA];
00113 
00114   /*the following is to deal with a possibly timing issue*/
00115   /*the communication occasionally breaks without this*/
00116   usleep(100000);
00117   for(i=0;i<MAXRETRY;i++) {
00118     if(rdLMSbyte(fd)==0x06)
00119     {
00120        break;
00121     }
00122   }
00123   buflen=rdLMSmsg(fd,ackmsglen-1,buf);
00124   return msgcmp(ackmsglen-1,ackmsg+1,buflen,buf);
00125 }
00126 
00127 
00128 
00129 /*set the communication speed and terminal properties*/
00130 bool SickLMS200::initLMS(const char *serialdev, struct termios *oldtio, int& fd)
00131 {
00132   struct termios newtio_struct, *newtio=&newtio_struct;
00133 
00134   fd = open(serialdev, O_RDWR | O_NOCTTY );
00135   if (fd <0) {
00136     log(Error)<< " SickLMS200 IOException."<<endlog();
00137     return false;
00138   }
00139 
00140   tcgetattr(fd,oldtio); /* save current port settings */
00141 
00142   /*after power up, the laser scanner will reset to 9600bps*/
00143   memset(newtio, 0, sizeof(struct termios));
00144   newtio->c_cflag = B9600 | CS8 | CLOCAL | CREAD;
00145   newtio->c_iflag = IGNPAR;
00146   newtio->c_oflag = 0;
00147 
00148   /* set input mode (non-canonical, no echo,...) */
00149   newtio->c_lflag = 0;
00150   newtio->c_cc[VTIME]    = 10;   /* inter-character timer unused */
00151   newtio->c_cc[VMIN]     = 255;   /* blocking read until 1 chars received */
00152   tcflush(fd, TCIFLUSH);
00153   tcsetattr(fd,TCSANOW,newtio);
00154 
00155 
00156   /* step to the 38400bps mode*/
00157   wtLMSmsg(fd,sizeof(PCLMS_B38400)/sizeof(uchar),PCLMS_B38400);
00158   if(!chkAck(fd,sizeof(LMSPC_CMD_ACK)/sizeof(uchar),LMSPC_CMD_ACK)){
00159     log(Error)<< " SickLMS200 BaudRateChangeException."<<endlog();
00160     throw BaudRateChangeException();
00161   }
00162 
00163   /* set the PC side as well*/
00164   close(fd);
00165   fd = open(serialdev, O_RDWR | O_NOCTTY );
00166   if (fd <0) {
00167     log(Error)<< " SickLMS200 IOException."<<endlog();
00168     return false;
00169   }
00170   newtio->c_cflag = B38400 | CS8 | CLOCAL | CREAD;
00171   tcflush(fd, TCIFLUSH);
00172   tcsetattr(fd,TCSANOW,newtio);
00173 
00174   return true;
00175 }
00176 
00177 
00178 /*set both the angular range and resolutions*/
00179 bool SickLMS200::setmode(int fd, int mode)
00180 {
00181   const uchar *msg, *ackmsg;
00182   int msglen, ackmsglen, unit, res;
00183 
00184   /*change the resolution*/
00185   res=mode & 0x3e;
00186   switch(res){
00187   case (RES_1_DEG | RANGE_100):
00188     msg=PCLMS_RES1;
00189     ackmsg=LMSPC_RES1_ACK;
00190     break;
00191   case (RES_0_5_DEG | RANGE_100):
00192     msg=PCLMS_RES2;
00193     ackmsg=LMSPC_RES2_ACK;
00194     break;
00195   case (RES_0_25_DEG | RANGE_100):
00196     msg=PCLMS_RES3;
00197     ackmsg=LMSPC_RES3_ACK;
00198     break;
00199   case (RES_1_DEG | RANGE_180):
00200     msg=PCLMS_RES4;
00201     ackmsg=LMSPC_RES4_ACK;
00202     break;
00203   case (RES_0_5_DEG | RANGE_180):
00204     msg=PCLMS_RES5;
00205     ackmsg=LMSPC_RES5_ACK;
00206     break;
00207   default:
00208     msg=PCLMS_RES1;
00209     ackmsg=LMSPC_RES1_ACK;
00210     log(Error)<< " SickLMS200 InvalidResolutionException."<<endlog();
00211     return false;
00212     break;
00213   }
00214 
00215   /*the following two line works only because msg and ackmsg are const uchar str*/
00216   msglen=sizeof(PCLMS_RES1)/sizeof(uchar);
00217   ackmsglen=sizeof(LMSPC_RES1_ACK)/sizeof(uchar);
00218   wtLMSmsg(fd,msglen,msg);
00219   if(!chkAck(fd,ackmsglen,ackmsg)){
00220     log(Error)<< " SickLMS200 ResolutionFailureException."<<endlog();
00221     return false;
00222   }
00223 
00224   /*change the measurement unit*/
00225   unit=mode & 0x1;
00226   /*may need to increase the timeout to 7sec here*/
00227   if(unit==MMMODE){
00228     msg=PCLMS_MM;
00229     ackmsg=LMSPC_MM_ACK;
00230   }else if(unit==CMMODE){
00231     msg=PCLMS_CM;
00232     ackmsg=LMSPC_CM_ACK;
00233   }
00234   /*invoking setting mode*/
00235   msglen=sizeof(PCLMS_SETMODE)/sizeof(uchar);
00236   ackmsglen=sizeof(LMSPC_CMD_ACK)/sizeof(uchar);
00237   wtLMSmsg(fd,msglen,PCLMS_SETMODE);
00238   if(!chkAck(fd,ackmsglen,LMSPC_CMD_ACK)){
00239     log(Error)<< " SickLMS200 ModeFailureException."<<endlog();
00240     return false;
00241   }
00242   /*the following two line works only because msg and ackmsg are const uchar str*/
00243   msglen=sizeof(PCLMS_MM)/sizeof(uchar);
00244   ackmsglen=sizeof(LMSPC_MM_ACK)/sizeof(uchar);
00245   wtLMSmsg(fd,msglen,msg);
00246   if(!chkAck(fd,ackmsglen,ackmsg)){
00247     log(Error)<< " SickLMS200 ModeFailureException."<<endlog();
00248     return false;
00249   }
00250   return true;
00251 }
00252 
00253 /*tell the scanner to enter the continuous measurement mode*/
00254 bool SickLMS200::startLMS(int fd)
00255 {
00256   log(Debug)<< " SickLMS200::startLMS entered."<<endlog();
00257   int ackmsglen;
00258 
00259   wtLMSmsg(fd,sizeof(PCLMS_START)/sizeof(uchar),PCLMS_START);
00260   ackmsglen=sizeof(LMSPC_CMD_ACK)/sizeof(uchar);
00261   return chkAck(fd,ackmsglen,LMSPC_CMD_ACK);
00262 }
00263 
00264 /*stop the continuous measurement mode*/
00265 bool SickLMS200::stopLMS(int fd)
00266 {
00267   int ackmsglen;
00268 
00269   wtLMSmsg(fd,sizeof(PCLMS_STOP)/sizeof(uchar),PCLMS_STOP);
00270   ackmsglen=sizeof(LMSPC_CMD_ACK)/sizeof(uchar);
00271   return chkAck(fd,ackmsglen,LMSPC_CMD_ACK);
00272 }
00273 
00274 
00275 bool SickLMS200::checkErrorMeasurement() {
00276   switch(meas_state & 0x07){
00277     case 0: return false;
00278     case 1: return false;
00279     case 2: return false;
00280     case 3: return true;
00281     case 4: {
00282       log(Error)<< " SickLMS200 FatalMeasurementException."<<endlog();
00283       return false;
00284     }
00285     default: {
00286       log(Error)<< " SickLMS200 FatalMeasurementException."<<endlog();
00287       return false;
00288     }
00289   }
00290 }
00291 
00292 bool SickLMS200::checkPlausible() {
00293     return (meas_state & 0xC0)==0;
00294 }
00295 
00296 
00297 
00298 /*reset terminal and transfer speed of laser scanner before quitting*/
00299 bool SickLMS200::resetLMS(int fd, struct termios *oldtio)
00300 {
00301   wtLMSmsg(fd,sizeof(PCLMS_B9600)/sizeof(uchar),PCLMS_B9600);
00302   tcflush(fd, TCIFLUSH);
00303   tcsetattr(fd,TCSANOW,oldtio);
00304   close(fd);
00305   return chkAck(fd,sizeof(LMSPC_CMD_ACK)/sizeof(uchar),LMSPC_CMD_ACK);
00306 }
00307 
00308 SickLMS200::SickLMS200(const char* _port, uchar _range_mode, uchar _res_mode, uchar _unit_mode) {
00309     port = _port;
00310     range_mode = _range_mode;
00311     res_mode   = _res_mode;
00312     unit_mode  = _unit_mode;
00313 }
00314 
00315 bool SickLMS200::start() {
00316     if (!initLMS(port,&oldtio,fd)) return false;
00317     if (!setmode(fd, range_mode | res_mode | unit_mode)) return false;
00318     return startLMS(fd);
00319 }
00320 bool SickLMS200::stop() {
00321     bool ret1 = stopLMS(fd);
00322     bool ret2 = resetLMS(fd,&oldtio);
00323 
00324     return ret1 && ret2;
00325 }
00326 bool SickLMS200::reset() {
00327   return resetLMS(fd,&oldtio);
00328 }
00329 
00330 SickLMS200::~SickLMS200() {
00331 }
00332 
00337 int SickLMS200::readMeasurement(uchar* buf,int& datalen) {
00338     if(rdLMSbyte(fd)!=STX) return -1;
00339     rdLMSbyte(fd); /*should be the ADR byte, ADR=0X80 here*/
00340     /*LEN refers to the packet length in bytes, datalen
00341       refers to the number of measurements, each of them are 16bits long*/
00342     rdLMSbyte(fd); /*should be the LEN low byte*/
00343     rdLMSbyte(fd); /*should be the LEN high byte*/
00344     rdLMSbyte(fd); /*should be the CMD byte, CMD=0xb0 in this mode*/
00345     datalen=rdLMSbyte(fd);
00346     datalen |= (rdLMSbyte(fd) & 0x1f) << 8; /*only lower 12 bits are significant*/
00347     datalen *= 2; /*each reading is 2 bytes*/
00348     rdLMSmsg(fd,datalen,buf);
00349     meas_state = rdLMSbyte(fd);
00350     rdLMSbyte(fd); /*should be CRC low byte*/
00351     rdLMSbyte(fd); /*should be CRC high byte*/
00352     return 0;
00353 }
00354 
00355 
00356 static SickLMS200* sick=0;
00357 static void SickLMS200_trap(int sig) {
00358     if (sick!=0) {
00359         sick->reset();
00360     }
00361 };
00362 
00363 bool registerSickLMS200SignalHandler() {
00364   return !(signal(SIGTERM, SickLMS200_trap) ==SIG_ERR || signal(SIGHUP, SickLMS200_trap) ==SIG_ERR ||
00365        signal(SIGINT, SickLMS200_trap) ==SIG_ERR || signal(SIGQUIT, SickLMS200_trap) ==SIG_ERR ||
00366        signal(SIGABRT, SickLMS200_trap) ==SIG_ERR);
00367 };
00368 
00369 
00370 Exception::Exception(const char* _descr):descr(_descr) {}
00371 const char* Exception::getDescription() { return descr; }
00372 
00373 BaudRateChangeException::BaudRateChangeException():Exception("Failure to set the baud rate at Sick side") {};
00374 
00375 InvalidResolutionException::InvalidResolutionException():Exception("Invalid resolution selected") {}
00376 
00377 ResolutionFailureException::ResolutionFailureException():Exception("Failure to set the resolution mode at Sick side") {}
00378 
00379 ModeFailureException::ModeFailureException():Exception("Failure to set the measurement mode at Sick side") {}
00380 
00381 StartFailureException::StartFailureException():Exception("Failure to start the Sick sensor") {}
00382 
00383 StopFailureException::StopFailureException():Exception("Failure to stop the Sick sensor") {}
00384 
00385 RegisterException::RegisterException():Exception("Failure to register a signal handler") {}
00386 
00387 IOException::IOException(const char* _descr):Exception(_descr) {}
00388 
00389 FatalMeasurementException::FatalMeasurementException():Exception("Fatal measurement exception") {}
00390 
00391 };
00392