URG04LXmm.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00036
00037
00038
00039
00040 #ifndef __URG04LXMM_H_
00041 #define __URG04LXMM_H_
00042
00043 #ifdef MIP_HOST_APPLE
00044 #include <applePatch.h>
00045 #endif
00046
00047 #include <assert.h>
00048 #include <errno.h>
00049 #include <fcntl.h>
00050 #include <fstream>
00051 #include <iostream>
00052 #include <poll.h>
00053 #include <signal.h>
00054 #include <stdlib.h>
00055 #include <string.h>
00056 #include <sys/types.h>
00057 #include <termios.h>
00058 #include <Time.h>
00059 #include <unistd.h>
00060
00061 #include <ExtString.h>
00062 #include <Scan.h>
00063 #include <socketUDP.h>
00064
00065 #include "RangeSens.h"
00066 #include "URG04LXenums.h"
00067 #include "remKhepComm.h"
00068
00069
00070
00071 namespace MipResources{
00072
00073
00075
00076
00087 class URG04LXmmConst
00088 {
00089 private:
00090 static const int _linRes=1;
00091 static const int _linRangeMin=20;
00092 static const int _linRangeMax2B=4096;
00093 static const int _linRangeMax3B=5600;
00094 static const int _angRes= (int)(( 360.0/1024 ) * ( MPIm/180 ));
00095 static const int _frontStep=384;
00096 static const int _rate=1;
00097 static const int _retries=20;
00098 static const int _bufSize=2048;
00099 Pose _URGpose;
00100
00101 Time _maxCmdTime;
00102
00103 const char* _dev;
00104
00105 ExtString _name;
00106 ExtString _protocol;
00107 ExtString _info;
00108 ExtString _params;
00109 ExtString _state;
00110 ExtString _laserON;
00111 ExtString _laserOFF;
00112 ExtString _timerON;
00113 ExtString _timerOFF;
00114 ExtString _reset;
00115 ExtString _lf;
00116
00117 public:
00119 URG04LXmmConst()
00120 {
00121 _dev="/dev/ttyACM0";
00122
00123 _URGpose = Pose ( 0.0,0.0,0.0 );
00124 _maxCmdTime=Time ( 10,0 );
00125 _name = "URG-04LX by Hokuyo Automatic Co. Ltd.\n";
00126 _protocol = "SCIP2.0\n";
00127 _info = "VV;MyInfo\n";
00128 _params = "PP;MyParam\n";
00129 _state = "II;MyState\n";
00130 _laserON = "BM;LaserOn\n";
00131 _laserOFF = "QT;LaserOff\n";
00132 _timerON = "TM0;TimerOn\n";
00133 _timerOFF = "TM2;TimerOff\n";
00134 _reset = "RS;Reset\n";
00135 _lf= "\n";
00136
00137
00138 }
00139
00140
00141
00142
00143
00145 const char* dev()
00146 {
00147 return _dev;
00148 }
00149
00151 static const int linRes()
00152 {
00153 return _linRes;
00154 }
00155
00157 static const int linRangeMin(){
00158 return _linRangeMin;
00159 }
00160
00162 static const int linRangeMax2B(){
00163 return _linRangeMax2B;
00164 }
00165
00167 static const int linRangeMax3B(){
00168 return _linRangeMax3B;
00169 }
00170
00172 static const int angRes(){
00173 return _angRes;
00174 }
00175
00177 int frontStep(){
00178 return _frontStep;
00179 }
00180
00182 static const int rate()
00183 {
00184 return _rate;
00185 }
00186
00188 static const int retries()
00189 {
00190 return _retries;
00191 }
00192
00194 static const int bufSize()
00195 {
00196 return _bufSize;
00197 }
00198
00200 Pose URGpose()
00201 {
00202 return _URGpose;
00203 }
00204
00206 Time maxCmdTime()
00207 {
00208 return _maxCmdTime;
00209 }
00210
00212 ExtString name()
00213 {
00214 return _name;
00215 }
00216
00218 char* protocol()
00219 {
00220 return ( char* ) _protocol.c_str();
00221 }
00222
00224 char* info()
00225 {
00226 return ( char* ) _info.c_str();
00227 }
00228
00230 char* params()
00231 {
00232 return ( char* ) _params.c_str();
00233 }
00234
00236 char* state()
00237 {
00238 return ( char* ) _state.c_str();
00239 }
00240
00242 char* laserON()
00243 {
00244 return ( char* ) _laserON.c_str();
00245 }
00246
00248 char* laserOFF()
00249 {
00250 return ( char* ) _laserOFF.c_str();
00251 }
00252
00254 char* timerON()
00255 {
00256 return ( char* ) _timerON.c_str();
00257 }
00258
00260 char* timerOFF()
00261 {
00262 return ( char* ) _timerOFF.c_str();
00263 }
00264
00266 char* reset()
00267 {
00268 return ( char* ) _reset.c_str();
00269 }
00270
00272 char* lf()
00273 {
00274 return ( char* ) _lf.c_str();
00275 }
00276 };
00277
00288 class URG04LXmmCommand : public MIPObject{
00289 private:
00290 URG04LXmmConst _URGConst;
00291
00292 const char* _device;
00293 FILE* _laserPort;
00294 int _fd;
00295 static const int timeout=0;
00296 bool _SCIP2;
00297 bool _firstBlock;
00298 bool _isFinished;
00299 bool _verboseOn;
00300 bool _log;
00301 bool _fdBad;
00302
00303
00304 ofstream _logFile;
00305
00307 enum _cmdFSMSstates
00308 {
00309 send,
00310 receive,
00311 check,
00312 store,
00313 recovery
00314 };
00315
00316 public:
00317
00319 URG04LXmmCommand();
00320
00322 ~URG04LXmmCommand();
00323
00326 string getObjectName() const {
00327 return "URG04LXmm";
00328 }
00329
00332 void setDevice( const char* device );
00333
00337 int writeCmd( char* s );
00338
00342 int readCmd ( char* s );
00343
00348 ExtString int2string ( int bytes, int value );
00349
00354 bool checkEcho ( ExtString command, ExtString response );
00355
00360 bool checkStatus ( ExtString command, ExtString response );
00361
00363 void changeProtocol();
00364
00366 void on();
00367
00369 void off();
00370
00372 void reset();
00373
00377 Time timeMatch ( ExtString testString );
00378
00384 bool localFSM ( char* _command, ExtString &_output );
00385
00386
00387
00388
00389
00391 void setSCIP2() {
00392 _SCIP2=true;
00393 }
00394
00396 void unsetSCIP2() {
00397 _SCIP2=false;
00398 }
00399
00401 void setFirstBlock() {
00402 _firstBlock=true;
00403 }
00404
00406 void setVerboseOn() {
00407 _verboseOn=true;
00408 }
00409
00411 void setLog() {
00412 _logFile.open( "logFile.txt",ios::trunc );
00413 _logFile.close();
00414 _logFile.open( "logFile.txt",ios::app );
00415 _log=true;
00416 }
00417
00419 void setfdBad() {
00420 _fdBad=true;
00421 }
00422
00424 void unsetFirstBlock() {
00425 _firstBlock=false;
00426 }
00427
00429 void setIsFinished() {
00430 _isFinished=true;
00431 }
00432
00434 void unsetIsFinished() {
00435 _isFinished=false;
00436 }
00437
00439 void unsetVerboseOn() {
00440 _verboseOn=false;
00441 }
00442
00444 void unsetLog() {
00445 _logFile.close();
00446 _log=false;
00447 }
00448
00450 void unsetfdBad() {
00451 _fdBad=false;
00452 }
00453
00454
00455
00456
00457
00459 const char* device()
00460 {
00461 return _device;
00462 }
00463
00465 FILE* laserPort()
00466 {
00467 return _laserPort;
00468 }
00469
00471 int fd()
00472 {
00473 return _fd;
00474 }
00475
00477 bool SCIP2()
00478 {
00479 return _SCIP2;
00480 }
00481
00483 bool firstBlock()
00484 {
00485 return _firstBlock;
00486 }
00487
00489 bool isFinished()
00490 {
00491 return _isFinished;
00492 }
00494 bool verboseOn()
00495 {
00496 return _verboseOn;
00497 }
00498
00500 bool log()
00501 {
00502 return _log;
00503 }
00504
00506 bool fdBad()
00507 {
00508 return _fdBad;
00509 }
00510 };
00511
00519 class URG04LXmmInfo
00520 {
00521 private:
00522
00523 ExtString _vendor;
00524 ExtString _product;
00525 ExtString _firmware;
00526 ExtString _protocol;
00527 ExtString _SN;
00528 bool _laserIsOn;
00529 ExtString _sensorState;
00530
00531 public:
00532
00534 URG04LXmmInfo();
00535
00536
00537
00538
00539
00541 void setVendor ( ExtString value )
00542 {
00543 _vendor=value;
00544 }
00545
00547 void setProduct ( ExtString value )
00548 {
00549 _product=value;
00550 }
00551
00553 void setFirmware ( ExtString value )
00554 {
00555 _firmware=value;
00556 }
00557
00559 void setProtocol ( ExtString value )
00560 {
00561 _protocol=value;
00562 }
00563
00565 void setSN ( ExtString value )
00566 {
00567 _SN=value;
00568 }
00569
00571 void setLaserState ( ExtString value )
00572 {
00573 if ( value=="ON" )
00574 _laserIsOn=true;
00575 else
00576 _laserIsOn=false;
00577 }
00578
00580 void setSensorState ( ExtString value )
00581 {
00582 _sensorState=value;
00583 }
00584
00585
00586
00587
00588
00590 ExtString vendor()
00591 {
00592 return _vendor;
00593 }
00594
00596 ExtString product()
00597 {
00598 return _product;
00599 }
00600
00602 ExtString firmware()
00603 {
00604 return _firmware;
00605 }
00606
00608 ExtString protocol()
00609 {
00610 return _protocol;
00611 }
00612
00614 ExtString SN()
00615 {
00616 return _SN;
00617 }
00618
00620 ExtString laserState()
00621 {
00622 if ( _laserIsOn )
00623 return "Laser in ON";
00624 else
00625 return "Laser in OFF";
00626 }
00627
00629 ExtString sensorState()
00630 {
00631 return _sensorState;
00632 }
00633 };
00634
00645 class URG04LXmmPar : public RangeSensmmPar
00646 {
00647 private:
00648 URG04LXmmConst _URGConst;
00649
00650 int _enc;
00651 int _firstStep;
00652 int _lastStep;
00653 int _cluster;
00654 int _scanInterval;
00655 int _scanNum;
00656 int _motorSpeed;
00657 int _baudRate;
00658
00659
00660
00661
00662
00663
00664
00665 Time _maxCmdTime;
00666
00668
00669
00670 public:
00672 URG04LXmmPar();
00673
00675 ~URG04LXmmPar();
00676
00677
00678
00679
00680
00684 void setEncoding ( int value )
00685 {
00686 _enc=value;
00687 }
00688
00691 void setFirstStep ( int value )
00692 {
00693 _firstStep=value;
00694 }
00695
00699 void setLastStep ( int value )
00700 {
00701 _lastStep=value;
00702 }
00703
00707 void setCluster ( int value )
00708 {
00709 _cluster=value;
00710 }
00711
00715 void setScanInterval ( int value )
00716 {
00717 _scanInterval=value;
00718 }
00719
00724 void setScanNum ( int value )
00725 {
00726 _scanNum=value;
00727 }
00728
00731 void setMotorSpeed ( int value )
00732 {
00733 _motorSpeed=value;
00734 }
00735
00738 void setBaudRate ( int value )
00739 {
00740 _baudRate=value;
00741 }
00742
00743
00744
00745
00746
00748 int encoding()
00749 {
00750 return _enc;
00751 }
00752
00754 int firstStep()
00755 {
00756 return _firstStep;
00757 }
00758
00760 int lastStep()
00761 {
00762 return _lastStep;
00763 }
00764
00766 int cluster()
00767 {
00768 return _cluster;
00769 }
00770
00772 int scanInterval()
00773 {
00774 return _scanInterval;
00775 }
00776
00778 int scanNum()
00779 {
00780 return _scanNum;
00781 }
00782
00784 int motorSpeed()
00785 {
00786 return _motorSpeed;
00787 }
00788
00790 int baudRate()
00791 {
00792 return _baudRate;
00793 }
00794 };
00795
00809 class URG04LXmm : public RangeSens{
00810 private:
00811
00812
00813
00814
00815
00816
00817
00818 vector<Time> _receiveTimes;
00819 vector<int> _receiveSizes;
00820
00821 Timer _scanSendTimer;
00822
00823 static const ResourcePlate _plate = RS_URG_RES;
00824
00825 void sendSingleFSMS(Time &_elapsed, char* _command);
00826 void sendMultiFSMS(Time &_elapsed, char* _command);
00827 void searchAnswerFSMS(Time &_elapsed);
00828 void getTimeFSMS();
00829 void receiveFSMS(Time &_elapsed);
00830 void storeDataFSMS(Time &_elapsed);
00831 void parseFSMS(Time &_elapsed, ExtString &_currentLine, ExtString &_strTime);
00832 void storeScanFSMS(Time &_elapsed);
00833 void resetFSMS(Time &_elapsed);
00834 void waitEchoFSMS(Time &_elapsed);
00835 void waitScanFSMS(Time &_elapsed);
00836 void recoveryFSMS(Time &_elapsed);
00837 void escapeFSMS();
00838
00839 protected:
00840
00841 ExtString _strCmd;
00842 ExtString _answer;
00843 char* _answerBuf;
00844 char* _answerBufptr;
00845 int _answerBufSize;
00846 bool _badScan;
00847 ExtString _temporary;
00848
00849 Thread _scanThread;
00850
00851 unsigned int _expectedLines;
00852 unsigned int _expectedChars;
00853 unsigned int _expectedBytes;
00854 unsigned int _alreadyRead;
00855 unsigned int _expectedRays;
00856 unsigned int _parsedLines;
00857 ExtString _parsingBits;
00858 ExtString _prevTStamp;
00859 Raymms _tempRays;
00860 Time _tStamp;
00861 unsigned int _totRays;
00862 unsigned int _totScans;
00863 unsigned int _lfcount;
00864 unsigned int _readAttempts;
00865 unsigned int _remainingScans;
00866
00867 bool _wait;
00868 bool _isContinuos;
00869 bool _echoMatch;
00870 bool _statusMatch;
00871 bool _tStampAcq;
00872 bool _checkLine;
00873 bool _scanComplete;
00874 bool _workDone;
00875 bool _verboseOn;
00876 bool _log;
00877
00878 Time _readDataTime;
00879 Time _waitNextScan;
00880 Time _FSMperiod;
00881 Time _waitEchoMatch;
00882 Timer _waitedScan;
00883 Timer _waitedEcho;
00884
00885 int _waitNextScanTimeusecs;
00886
00887 ofstream _logFile;
00888
00891 void parse2bytes ( ExtString _currentLine );
00892
00895 void parse3bytes ( ExtString& _currentLine );
00896
00898 void updateInfo();
00899
00901 bool buildMultiScan ( ExtString& command );
00902
00904 bool buildSingleScan ( ExtString& command );
00905
00907 int readData ( ExtString command,ExtString & rbuf,int how );
00908
00910 int readData ( ExtString command,int how );
00911
00914 void changeMotor ( int value );
00915
00918 void changeBuad ( int value );
00919
00920 SenderUDP* _sender;
00921 bool _sendScan;
00923 char _sendingBuffer[10000];
00924 int _printedBuffer;
00925
00926
00927 public:
00928 Timer _FSMtimer;
00929
00930 scanWorkState SWstate;
00931 FSMSstates _state;
00932 Scan _myScan;
00933
00934 bool _canDraw;
00935
00936 URG04LXmmPar *_URGpar;
00937 URG04LXmmConst _URGConst;
00938 URG04LXmmInfo *_URGinfo;
00939 URG04LXmmCommand *_cmd;
00940
00942 URG04LXmm(int argc, const char* argv[]);
00943
00945 URG04LXmm(int argc, const char* argv[], SenderUDP* sender);
00946
00948 ~URG04LXmm();
00949
00951 ResourcePlate getPlate() const {
00952 return _plate;
00953 }
00954
00955 void setWaitNextScanTimeusecs(int t){
00956 _waitNextScanTimeusecs = t;
00957 }
00958
00959
00960
00961
00963 void setSender(SenderUDP* sender) {
00964 _sender = sender;
00965 }
00966
00968 void setWait()
00969 {
00970 _wait=true;
00971 }
00972
00974 void setIsContinuos()
00975 {
00976 _isContinuos=true;
00977 }
00978
00980 void setEchoMatch()
00981 {
00982 _echoMatch=true;
00983 }
00984
00986 void setStatusMatch()
00987 {
00988 _statusMatch=true;
00989 }
00990
00992 void setTStampAcq()
00993 {
00994 _tStampAcq=true;
00995 }
00996
00998 void setScanComplete()
00999 {
01000 _scanComplete=true;
01001 }
01002
01004 void setCheckLine()
01005 {
01006 _checkLine=true;
01007 }
01008
01010 void setWorkDone()
01011 {
01012 _workDone=true;
01013 }
01014
01016 void setVerboseOn()
01017 {
01018 _cmd->setVerboseOn();
01019 _verboseOn=true;
01020 }
01021
01023 void setLog()
01024 {
01025 _logFile.open("logFile.txt",ios_base::app);
01026 _cmd->setLog();
01027 _log=true;
01028 }
01029
01031 void unsetWait()
01032 {
01033 _wait=false;
01034 }
01035
01037 void unsetIsContinuos()
01038 {
01039 _isContinuos=false;
01040 }
01041
01043 void unsetEchoMatch()
01044 {
01045 _echoMatch=false;
01046 }
01047
01049 void unsetStatusMatch()
01050 {
01051 _statusMatch=false;
01052 }
01053
01055 void unsetTStampAcq()
01056 {
01057 _tStampAcq=false;
01058 }
01059
01061 void unsetScanComplete()
01062 {
01063 _scanComplete=false;
01064 }
01065
01067 void unsetCheckLine()
01068 {
01069 _checkLine=false;
01070 }
01071
01073 void unsetWorkDone()
01074 {
01075 _workDone=false;
01076 }
01077
01079 void unsetVerboseOn()
01080 {
01081 _cmd->unsetVerboseOn();
01082 _verboseOn=false;
01083 }
01084
01086 void unsetLog()
01087 {
01088 _cmd->unsetLog();
01089 _log=false;
01090 }
01091
01092
01093
01094
01095
01097 int totRays()
01098 {
01099 return _totRays;
01100 }
01101
01103 int totScans()
01104 {
01105 return _totScans;
01106 }
01107
01109 bool wait()
01110 {
01111 return _wait;
01112 }
01113
01115 bool isContinuos()
01116 {
01117 return _isContinuos;
01118 }
01119
01121 bool echoMatch()
01122 {
01123 return _echoMatch;
01124 }
01125
01127 bool statusMatch()
01128 {
01129 return _statusMatch;
01130 }
01131
01133 bool tStampAcq()
01134 {
01135 return _tStampAcq;
01136 }
01137
01139 bool scanComplete()
01140 {
01141 return _scanComplete;
01142 }
01143
01145 bool checkLine()
01146 {
01147 return _checkLine;
01148 }
01149
01151 bool workDone()
01152 {
01153 return _workDone;
01154 }
01155
01157 bool verboseOn()
01158 {
01159 return _verboseOn;
01160 }
01161
01163 bool log()
01164 {
01165 return _log;
01166 }
01167
01169 bool updatedScan ( Scan& s );
01170
01172
01173
01174
01175
01176
01177
01179 Scan _singleScan();
01180
01181
01182
01184 void mySingleScan()
01185 {
01186 _state=sendSingle;
01187 _isContinuos=false;
01188 };
01189
01192 void _startScan ( unsigned int num );
01193
01195 void _stopScan();
01196
01198 void defFSM ();
01199
01200 };
01201
01202
01203
01204
01205 };
01206
01207
01208 #endif
01209
01210
01211
01212