URG04LX.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 #ifndef __URG04LX_H_
00040 #define __URG04LX_H_
00041
00042 #ifdef MIP_HOST_APPLE
00043 #include <applePatch.h>
00044 #endif
00045
00046 #include <assert.h>
00047 #include <errno.h>
00048 #include <fcntl.h>
00049 #include <fstream>
00050 #include <iostream>
00051 #include <poll.h>
00052 #include <signal.h>
00053 #include <stdlib.h>
00054 #include <string.h>
00055 #include <sys/types.h>
00056 #include <termios.h>
00057 #include <Time.h>
00058 #include <unistd.h>
00059
00060 #include <ExtString.h>
00061 #include <socketUDP.h>
00062
00063 #include "RangeSens.h"
00064 #include "URG04LXenums.h"
00065
00066
00068
00069 namespace MipResources{
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00107 class URG04LXConst{
00108 private:
00109 static const Decimal _linRes;
00110 static const Decimal _linRangeMin;
00111 static const Decimal _linRangeMax2B;
00112 static const Decimal _linRangeMax3B;
00113 static const Decimal _angRes;
00114 static const int _frontStep=384;
00115 static const int _rate=1;
00116 static const int _retries=20;
00117 static const int _bufSize=2048;
00118 Pose _URGpose;
00120 Time _maxCmdTime;
00122 const char* _dev;
00124 ExtString _name;
00125 ExtString _protocol;
00126 ExtString _info;
00127 ExtString _params;
00128 ExtString _state;
00129 ExtString _laserON;
00130 ExtString _laserOFF;
00131 ExtString _timerON;
00132 ExtString _timerOFF;
00133 ExtString _reset;
00134 ExtString _lf;
00136 public:
00138 URG04LXConst(){
00139 _dev="/dev/ttyACM0";
00140
00141 _URGpose = Pose(0.0,0.0,0.0);
00142 _maxCmdTime=Time(10,0);
00143 _name = "URG-04LX by Hokuyo Automatic Co. Ltd.\n";
00144 _protocol = "SCIP2.0\n";
00145 _info = "VV;MyInfo\n";
00146 _params = "PP;MyParam\n";
00147 _state = "II;MyState\n";
00148 _laserON = "BM;LaserOn\n";
00149 _laserOFF = "QT;LaserOff\n";
00150 _timerON = "TM0;TimerOn\n";
00151 _timerOFF = "TM2;TimerOff\n";
00152 _reset = "RS;Reset\n";
00153 _lf= "\n";
00154
00155
00156 }
00157
00158
00159
00160
00161
00163 const char* dev(){
00164 return _dev;
00165 }
00166
00168 static const Decimal linRes(){
00169 return _linRes;
00170 }
00171
00173 static const Decimal linRangeMin(){
00174 return _linRangeMin;
00175 }
00176
00178 static const Decimal linRangeMax2B(){
00179 return _linRangeMax2B;
00180 }
00181
00183 static const Decimal linRangeMax3B(){
00184 return _linRangeMax3B;
00185 }
00186
00188 static const Decimal angRes(){
00189 return _angRes;
00190 }
00191
00193 int frontStep(){
00194 return _frontStep;
00195 }
00196
00198 static const int rate(){
00199 return _rate;
00200 }
00201
00203 static const int retries(){
00204 return _retries;
00205 }
00206
00208 static const int bufSize(){
00209 return _bufSize;
00210 }
00211
00213 Pose URGpose(){
00214 return _URGpose;
00215 }
00216
00218 Time maxCmdTime(){
00219 return _maxCmdTime;
00220 }
00221
00223 ExtString name(){
00224 return _name;
00225 }
00226
00228 char* protocol(){
00229 return(char*) _protocol.c_str();
00230 }
00231
00233 char* info(){
00234 return(char*) _info.c_str();
00235 }
00236
00238 char* params(){
00239 return(char*) _params.c_str();
00240 }
00241
00243 char* state(){
00244 return(char*) _state.c_str();
00245 }
00246
00248 char* laserON(){
00249 return(char*) _laserON.c_str();
00250 }
00251
00253 char* laserOFF(){
00254 return(char*) _laserOFF.c_str();
00255 }
00256
00258 char* timerON(){
00259 return(char*) _timerON.c_str();
00260 }
00261
00263 char* timerOFF(){
00264 return(char*) _timerOFF.c_str();
00265 }
00266
00268 char* reset(){
00269 return(char*) _reset.c_str();
00270 }
00271
00273 char* lf(){
00274 return(char*) _lf.c_str();
00275 }
00276 };
00277
00288 class URG04LXCommand : public MIPObject{
00289 private:
00290 URG04LXConst _URGConst;
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;
00307 enum _cmdFSMSstates{
00308 send,
00309 receive,
00310 check,
00311 store,
00312 recovery
00313 };
00314
00315 public:
00316
00318 URG04LXCommand(){
00319
00320 _device=_URGConst.dev();
00321 _fd=::open(_device, O_RDWR|O_SYNC|O_NOCTTY);
00322 if (_fd==-1){
00323 fatal("Device not reachable, turn it on or add the option -mobileRobLaser 0");
00324 setfdBad();
00325 return;
00326 }
00327 unsetfdBad();
00328 _laserPort=fopen(_device,"r+");
00329 unsetSCIP2();
00330 unsetFirstBlock();
00331 unsetIsFinished();
00332 unsetVerboseOn();
00333 unsetLog();
00334
00335 }
00336
00338 ~URG04LXCommand(){
00339 if (_fd!=-1){
00340 if (log())
00341 _logFile.close();
00342 fclose(_laserPort);
00343 }
00344
00345 }
00346
00349 string getObjectName() const {
00350 return "URG04LX";
00351 }
00352
00355 void setDevice (const char* device){
00356 fclose(_laserPort);
00357 open(device, O_RDWR|O_SYNC|O_NOCTTY);
00358 _device=device;
00359 _fd=::open(_device, O_RDWR|O_SYNC|O_NOCTTY);
00360 _laserPort=fopen(_device,"r+");
00361 }
00362
00366 int writeCmd(char* s);
00367
00371 int readCmd(char* s);
00372
00377 ExtString int2string(int bytes, int value);
00378
00383 bool checkEcho(ExtString command, ExtString response);
00384
00389 bool checkStatus(ExtString command, ExtString response);
00390
00392 void changeProtocol();
00393
00395 void on();
00396
00398 void off();
00399
00401 void reset();
00402
00406 Time timeMatch(ExtString testString);
00407
00413 bool localFSM(char* _command, ExtString &_output);
00414
00415
00416
00417
00418
00420 void setSCIP2(){
00421 _SCIP2=true;
00422 }
00423
00425 void unsetSCIP2(){
00426 _SCIP2=false;
00427 }
00428
00430 void setFirstBlock(){
00431 _firstBlock=true;
00432 }
00433
00435 void setVerboseOn(){
00436 _verboseOn=true;
00437 }
00438
00440 void setLog(){
00441 _logFile.open( "logFile.txt",ios::trunc);
00442 _logFile.close();
00443 _logFile.open( "logFile.txt",ios::app);
00444 _log=true;
00445 }
00446
00448 void setfdBad(){
00449 _fdBad=true;
00450 }
00451
00453 void unsetFirstBlock(){
00454 _firstBlock=false;
00455 }
00456
00458 void setIsFinished(){
00459 _isFinished=true;
00460 }
00461
00463 void unsetIsFinished(){
00464 _isFinished=false;
00465 }
00466
00468 void unsetVerboseOn(){
00469 _verboseOn=false;
00470 }
00471
00473 void unsetLog(){
00474 _logFile.close();
00475 _log=false;
00476 }
00477
00479 void unsetfdBad(){
00480 _fdBad=false;
00481 }
00482
00483
00484
00485
00486
00488 const char* device(){
00489 return _device;
00490 }
00491
00493 FILE* laserPort(){
00494 return _laserPort;
00495 }
00496
00498 int fd(){
00499 return _fd;
00500 }
00501
00503 bool SCIP2(){
00504 return _SCIP2;
00505 }
00506
00508 bool firstBlock(){
00509 return _firstBlock;
00510 }
00511
00513 bool isFinished(){
00514 return _isFinished;
00515 }
00517 bool verboseOn(){
00518 return _verboseOn;
00519 }
00520
00522 bool log(){
00523 return _log;
00524 }
00525
00527 bool fdBad(){
00528 return _fdBad;
00529 }
00530 };
00531
00539 class URG04LXInfo{
00540 private:
00541
00542 ExtString _vendor;
00543 ExtString _product;
00544 ExtString _firmware;
00545 ExtString _protocol;
00546 ExtString _SN;
00547 bool _laserIsOn;
00548 ExtString _sensorState;
00550 public:
00551
00553 URG04LXInfo();
00554
00555
00556
00557
00558
00560 void setVendor(ExtString value){
00561 _vendor=value;
00562 }
00563
00565 void setProduct(ExtString value){
00566 _product=value;
00567 }
00568
00570 void setFirmware (ExtString value){
00571 _firmware=value;
00572 }
00573
00575 void setProtocol(ExtString value){
00576 _protocol=value;
00577 }
00578
00580 void setSN(ExtString value){
00581 _SN=value;
00582 }
00583
00585 void setLaserState(ExtString value){
00586 if(value=="ON")
00587 _laserIsOn=true;
00588 else
00589 _laserIsOn=false;
00590 }
00591
00593 void setSensorState(ExtString value){
00594 _sensorState=value;
00595 }
00596
00597
00598
00599
00600
00602 ExtString vendor(){
00603 return _vendor;
00604 }
00605
00607 ExtString product(){
00608 return _product;
00609 }
00610
00612 ExtString firmware(){
00613 return _firmware;
00614 }
00615
00617 ExtString protocol(){
00618 return _protocol;
00619 }
00620
00622 ExtString SN(){
00623 return _SN;
00624 }
00625
00627 ExtString laserState(){
00628 if(_laserIsOn)
00629 return "Laser in ON";
00630 else
00631 return "Laser in OFF";
00632 }
00633
00635 ExtString sensorState(){
00636 return _sensorState;
00637 }
00638 };
00639
00650 class URG04LXPar : public RangeSensPar{
00651 private:
00652 URG04LXConst _URGConst;
00654 int _enc;
00655 int _firstStep;
00656 int _lastStep;
00657 int _cluster;
00658 int _scanInterval;
00659 int _scanNum;
00660 int _motorSpeed;
00661 int _baudRate;
00663 Time _maxCmdTime;
00665
00666 void setParam();
00667
00668 public:
00670 URG04LXPar();
00671
00673 ~URG04LXPar();
00674
00675
00676
00677
00678
00682 void setEncoding(int value){
00683 _enc=value;
00684 }
00685
00688 void setFirstStep(int value){
00689 _firstStep=value;
00690 }
00691
00695 void setLastStep(int value){
00696 _lastStep=value;
00697 }
00698
00702 void setCluster(int value){
00703 _cluster=value;
00704 }
00705
00709 void setScanInterval(int value){
00710 _scanInterval=value;
00711 }
00712
00717 void setScanNum(int value){
00718 _scanNum=value;
00719 }
00720
00723 void setMotorSpeed(int value){
00724 _motorSpeed=value;
00725 }
00726
00729 void setBaudRate(int value){
00730 _baudRate=value;
00731 }
00732
00733
00734
00735
00736
00738 int encoding(){
00739 return _enc;
00740 }
00741
00743 int firstStep(){
00744 return _firstStep;
00745 }
00746
00748 int lastStep(){
00749 return _lastStep;
00750 }
00751
00753 int cluster(){
00754 return _cluster;
00755 }
00756
00758 int scanInterval(){
00759 return _scanInterval;
00760 }
00761
00763 int scanNum(){
00764 return _scanNum;
00765 }
00766
00768 int motorSpeed(){
00769 return _motorSpeed;
00770 }
00771
00773 int baudRate(){
00774 return _baudRate;
00775 }
00776 };
00777
00791 class URG04LX : public RangeSens{
00792 private:
00793 static const ResourcePlate _plate = RS_URG_RES;
00794 protected:
00795
00796 ExtString _strCmd;
00797 ExtString _answer;
00798 ExtString _temporary;
00800 Thread _scanThread;
00802 unsigned int _expectedLines;
00803 unsigned int _expectedChars;
00804 unsigned int _expectedBytes;
00805 unsigned int _alreadyRead;
00806 unsigned int _expectedRays;
00807 unsigned int _parsedLines;
00808 ExtString _parsingBits;
00809 ExtString _prevTStamp;
00810 Rays _tempRays;
00811 Time _tStamp;
00812 unsigned int _totRays;
00813 unsigned int _totScans;
00814 unsigned int _lfcount;
00815 unsigned int _readAttempts;
00816 unsigned int _remainingScans;
00818 bool _wait;
00819 bool _isContinuos;
00820 bool _echoMatch;
00821 bool _statusMatch;
00822 bool _tStampAcq;
00823 bool _checkLine;
00824 bool _scanComplete;
00825 bool _workDone;
00826 bool _verboseOn;
00827 bool _log;
00829 Time _readDataTime;
00830 Time _waitNextScan;
00831 Time _FSMperiod;
00832 Time _waitEchoMatch;
00833 Timer _waitedScan;
00834 Timer _waitedEcho;
00836
00837
00838 ofstream _logFile;
00840
00841
00842 void parse2bytes(ExtString _currentLine);
00843
00846 void parse3bytes(ExtString& _currentLine);
00847
00849 void updateInfo();
00850
00852 bool buildMultiScan(ExtString& command);
00853
00855 bool buildSingleScan(ExtString& command);
00856
00858 int readData(ExtString command,ExtString & rbuf,int how);
00859
00862 void changeMotor(int value);
00863
00866 void changeBuad(int value);
00867
00868 public:
00869 Timer _FSMtimer;
00870
00871 scanWorkState SWstate;
00872 FSMSstates _state;
00873 Scan _myScan;
00874
00875 bool _canDraw;
00876
00877 URG04LXPar *_URGpar;
00878 URG04LXConst _URGConst;
00879 URG04LXInfo *_URGinfo;
00880 URG04LXCommand *_cmd;
00882
00883 URG04LX(int argc, const char* argv[]);
00884
00886 ~URG04LX();
00887
00889 ResourcePlate getPlate() const{
00890 return _plate;
00891 }
00892
00893
00894
00895
00896
00898 void setWait(){
00899 _wait=true;
00900 }
00901
00903 void setIsContinuos(){
00904 _isContinuos=true;
00905 }
00906
00908 void setEchoMatch(){
00909 _echoMatch=true;
00910 }
00911
00913 void setStatusMatch(){
00914 _statusMatch=true;
00915 }
00916
00918 void setTStampAcq(){
00919 _tStampAcq=true;
00920 }
00921
00923 void setScanComplete(){
00924 _scanComplete=true;
00925 }
00926
00928 void setCheckLine(){
00929 _checkLine=true;
00930 }
00931
00933 void setWorkDone(){
00934 _workDone=true;
00935 }
00936
00938 void setVerboseOn(){
00939 _cmd->setVerboseOn();
00940 _verboseOn=true;
00941 }
00942
00944 void setLog(){
00945 _logFile.open("logFile.txt",ios_base::app);
00946 _cmd->setLog();
00947 _log=true;
00948 }
00949
00951 void unsetWait(){
00952 _wait=false;
00953 }
00954
00956 void unsetIsContinuos(){
00957 _isContinuos=false;
00958 }
00959
00961 void unsetEchoMatch(){
00962 _echoMatch=false;
00963 }
00964
00966 void unsetStatusMatch(){
00967 _statusMatch=false;
00968 }
00969
00971 void unsetTStampAcq(){
00972 _tStampAcq=false;
00973 }
00974
00976 void unsetScanComplete(){
00977 _scanComplete=false;
00978 }
00979
00981 void unsetCheckLine(){
00982 _checkLine=false;
00983 }
00984
00986 void unsetWorkDone(){
00987 _workDone=false;
00988 }
00989
00991 void unsetVerboseOn(){
00992 _cmd->unsetVerboseOn();
00993 _verboseOn=false;
00994 }
00995
00997 void unsetLog(){
00998 _cmd->unsetLog();
00999 _log=false;
01000 }
01001
01002
01003
01004
01005
01007 int totRays(){
01008 return _totRays;
01009 }
01010
01012 int totScans(){
01013 return _totScans;
01014 }
01015
01017 bool wait(){
01018 return _wait;
01019 }
01020
01022 bool isContinuos(){
01023 return _isContinuos;
01024 }
01025
01027 bool echoMatch(){
01028 return _echoMatch;
01029 }
01030
01032 bool statusMatch(){
01033 return _statusMatch;
01034 }
01035
01037 bool tStampAcq(){
01038 return _tStampAcq;
01039 }
01040
01042 bool scanComplete(){
01043 return _scanComplete;
01044 }
01045
01047 bool checkLine(){
01048 return _checkLine;
01049 }
01050
01052 bool workDone(){
01053 return _workDone;
01054 }
01055
01057 bool verboseOn(){
01058 return _verboseOn;
01059 }
01060
01062 bool log(){
01063 return _log;
01064 }
01065
01067 bool updatedScan(Scan& s);
01068
01070 bool storedScan(Scan& s,Time timeout);
01071
01072
01073
01074
01075
01077 Scan _singleScan();
01078
01079
01080
01082 void mySingleScan(){
01083 _state=sendSingle;
01084 _isContinuos=false;
01085 };
01086
01089 void _startScan(unsigned int num);
01090
01092 void _stopScan();
01093
01095 void defFSM ();
01096
01097 };
01098
01099
01100
01101
01102 };
01103
01104
01105 #endif
01106
01107
01108
01109
01110