URG04LXmm.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP 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 3 of the License, or
// (at your option) any later version.
//
// MIP 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 MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------






#ifndef __URG04LXMM_H_
#define __URG04LXMM_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <fstream>
#include <iostream>
#include <poll.h>
#include <signal.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <termios.h>
#include <Time.h>
#include <unistd.h>

#include <ExtString.h>
#include <Scan.h>
#include <socketUDP.h>

#include "RangeSens.h"
#include "URG04LXenums.h"
#include "remKhepComm.h"



namespace MipResources{


/* @{ */

class URG04LXmmConst
{
 private:
  static const int _linRes=1;                   
  static const int _linRangeMin=20;                
  static const int _linRangeMax2B=4096;              
  static const int _linRangeMax3B=5600;              
  static const int _angRes= (int)(( 360.0/1024 ) * ( MPIm/180 )); 
  static const int _frontStep=384;                
  static const int _rate=1;                    
  static const int _retries=20;                  
  static const int _bufSize=2048;                 
  Pose _URGpose;                         
  
  Time _maxCmdTime;   
  
  const char* _dev;   
  
  ExtString _name;   
  ExtString _protocol; 
  ExtString _info;   
  ExtString _params;  
  ExtString _state;   
  ExtString _laserON;  
  ExtString _laserOFF; 
  ExtString _timerON;  
  ExtString _timerOFF; 
  ExtString _reset;   
  ExtString _lf;    
  
 public:
  URG04LXmmConst()
  {
   _dev="/dev/ttyACM0";
   
   _URGpose = Pose ( 0.0,0.0,0.0 );
   _maxCmdTime=Time ( 10,0 );
   _name =     "URG-04LX by Hokuyo Automatic Co. Ltd.\n";
   _protocol =   "SCIP2.0\n";
   _info =     "VV;MyInfo\n";
   _params =    "PP;MyParam\n";
   _state =     "II;MyState\n";
   _laserON =    "BM;LaserOn\n";
   _laserOFF =   "QT;LaserOff\n";
   _timerON =    "TM0;TimerOn\n";
   _timerOFF =   "TM2;TimerOff\n";
   _reset =     "RS;Reset\n";
   _lf=      "\n";
   
//    _logDest=    "logFile.txt";
  }

/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  
  const char* dev()
  {
   return _dev;
  }
  
  static const int linRes()
  {
   return _linRes;
  }

  static const int linRangeMin(){
   return _linRangeMin;
  }

  static const int linRangeMax2B(){
   return _linRangeMax2B;
  }

  static const int linRangeMax3B(){
   return _linRangeMax3B;
  }

  static const int angRes(){
   return _angRes;
  }

  int frontStep(){
   return _frontStep;
  }
  
  static const int rate()
  {
   return _rate;
  }

  static const int retries()
  {
   return _retries;
  }

  static const int bufSize()
  {
   return _bufSize;
  }

  Pose URGpose()
  {
   return _URGpose;
  }

  Time maxCmdTime()
  {
   return _maxCmdTime;
  }

  ExtString name()
  {
   return _name;
  }

  char* protocol()
  {
   return ( char* ) _protocol.c_str();
  }

  char* info()
  {
   return ( char* ) _info.c_str();
  }

  char* params()
  {
   return ( char* ) _params.c_str();
  }

  char* state()
  {
   return ( char* ) _state.c_str();
  }

  char* laserON()
  {
   return ( char* ) _laserON.c_str();
  }

  char* laserOFF()
  {
   return ( char* ) _laserOFF.c_str();
  }

  char* timerON()
  {
   return ( char* ) _timerON.c_str();
  }

  char* timerOFF()
  {
   return ( char* ) _timerOFF.c_str();
  }

  char* reset()
  {
   return ( char* ) _reset.c_str();
  }

  char* lf()
  {
   return ( char* ) _lf.c_str();
  }
};

class URG04LXmmCommand : public MIPObject{
 private:
  URG04LXmmConst _URGConst;   
  
  const char* _device;     
  FILE* _laserPort;       
  int _fd;           
  static const int timeout=0;  
  bool _SCIP2;         
  bool _firstBlock;       
  bool _isFinished;       
  bool _verboseOn;       
  bool _log;          
  bool _fdBad;
  
  
  ofstream _logFile; 
  
  enum _cmdFSMSstates
  {
   send,   
   receive, 
   check,  
   store,  
   recovery 
  };
  
 public:
  
  URG04LXmmCommand();
  
  ~URG04LXmmCommand();
  
  string getObjectName() const {
   return "URG04LXmm";
  }
  
  void setDevice( const char* device );
  
  int writeCmd( char* s );
  
  int readCmd ( char* s );
  
  ExtString int2string ( int bytes, int value );
  
  bool checkEcho ( ExtString command, ExtString response );
  
  bool checkStatus ( ExtString command, ExtString response );
  
  void changeProtocol();
  
  void on();
  
  void off();
  
  void reset();
  
  Time timeMatch ( ExtString testString );
  
  bool localFSM ( char* _command, ExtString &_output );
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  
  void setSCIP2() {
   _SCIP2=true;
  }
  
  void unsetSCIP2() {
   _SCIP2=false;
  }
  
  void setFirstBlock() {
   _firstBlock=true;
  }
  
  void setVerboseOn() {
   _verboseOn=true;
  }
  
  void setLog() {
   _logFile.open( "logFile.txt",ios::trunc );
   _logFile.close();
   _logFile.open( "logFile.txt",ios::app );
   _log=true;
  }
  
  void setfdBad() {
   _fdBad=true;
  }
  
  void unsetFirstBlock() {
   _firstBlock=false;
  }

  void setIsFinished() {
   _isFinished=true;
  }

  void unsetIsFinished() {
   _isFinished=false;
  }
  
  void unsetVerboseOn() {
   _verboseOn=false;
  }
  
  void unsetLog() {
   _logFile.close();
   _log=false;
  }
  
  void unsetfdBad() {
   _fdBad=false;
  }

/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/

  const char* device()
  {
   return _device;
  }

  FILE* laserPort()
  {
   return _laserPort;
  }

  int fd()
  {
   return _fd;
  }

  bool SCIP2()
  {
   return _SCIP2;
  }

  bool firstBlock()
  {
   return _firstBlock;
  }

  bool isFinished()
  {
   return _isFinished;
  }
  bool verboseOn()
  {
   return _verboseOn;
  }
  
  bool log()
  {
   return _log;
  }
  
  bool fdBad()
  {
   return _fdBad;
  }
};

class URG04LXmmInfo
{
 private:

  ExtString _vendor;   
  ExtString _product;   
  ExtString _firmware;  
  ExtString _protocol;  
  ExtString _SN;     
  bool _laserIsOn;    
  ExtString _sensorState; 

 public:

  URG04LXmmInfo();

/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/

  void setVendor ( ExtString value )
  {
   _vendor=value;
  }

  void setProduct ( ExtString value )
  {
   _product=value;
  }

  void setFirmware ( ExtString value )
  {
   _firmware=value;
  }

  void setProtocol ( ExtString value )
  {
   _protocol=value;
  }

  void setSN ( ExtString value )
  {
   _SN=value;
  }

  void setLaserState ( ExtString value )
  {
   if ( value=="ON" )
    _laserIsOn=true;
   else
    _laserIsOn=false;
  }

  void setSensorState ( ExtString value )
  {
   _sensorState=value;
  }
  
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/

  ExtString vendor()
  {
   return _vendor;
  }

  ExtString product()
  {
   return _product;
  }

  ExtString firmware()
  {
   return _firmware;
  }

  ExtString protocol()
  {
   return _protocol;
  }

  ExtString SN()
  {
   return _SN;
  }

  ExtString laserState()
  {
   if ( _laserIsOn )
    return "Laser in ON";
   else
    return "Laser in OFF";
  }

  ExtString sensorState()
  {
   return _sensorState;
  }
};

class URG04LXmmPar : public RangeSensmmPar
{
 private:
  URG04LXmmConst _URGConst; 

  int _enc;      
  int _firstStep;   
  int _lastStep;   
  int _cluster;    
  int _scanInterval; 
  int _scanNum;    
  int _motorSpeed;  
  int _baudRate;   
  
  // param for efficency
//   _expectedRays;
//   _expectedLines;
//   _expectedChars;
//   _expectedBytes;

  Time _maxCmdTime; 

//   void setParam();

  public:
  URG04LXmmPar();

  ~URG04LXmmPar();

/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/

  void setEncoding ( int value )
  {
   _enc=value;
  }

  void setFirstStep ( int value )
  {
   _firstStep=value;
  }

  void setLastStep ( int value )
  {
   _lastStep=value;
  }

  void setCluster ( int value )
  {
   _cluster=value;
  }

  void setScanInterval ( int value )
  {
   _scanInterval=value;
  }

  void setScanNum ( int value )
  {
   _scanNum=value;
  }

  void setMotorSpeed ( int value )
  {
   _motorSpeed=value;
  }

  void setBaudRate ( int value )
  {
   _baudRate=value;
  }
  
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  
  int encoding()
  {
   return _enc;
  }

  int firstStep()
  {
   return _firstStep;
  }

  int lastStep()
  {
   return _lastStep;
  }

  int cluster()
  {
   return _cluster;
  }

  int scanInterval()
  {
   return _scanInterval;
  }

  int scanNum()
  {
   return _scanNum;
  }

  int motorSpeed()
  {
   return _motorSpeed;
  }

  int baudRate()
  {
   return _baudRate;
  }
};

class URG04LXmm : public RangeSens{
 private:
  
  
//   Timer TIMERONE;
//   long int TEMPODATARARE;
//   long int RECTIME;
//   int VALUE;
  
  vector<Time> _receiveTimes;
  vector<int> _receiveSizes;
  
  Timer _scanSendTimer;
  
  static const ResourcePlate _plate = RS_URG_RES;
  
  void sendSingleFSMS(Time &_elapsed, char* _command);
  void sendMultiFSMS(Time &_elapsed, char* _command);
  void searchAnswerFSMS(Time &_elapsed);
  void getTimeFSMS();
  void receiveFSMS(Time &_elapsed);
  void storeDataFSMS(Time &_elapsed);
  void parseFSMS(Time &_elapsed, ExtString &_currentLine, ExtString &_strTime);
  void storeScanFSMS(Time &_elapsed);
  void resetFSMS(Time &_elapsed);
  void waitEchoFSMS(Time &_elapsed);
  void waitScanFSMS(Time &_elapsed);
  void recoveryFSMS(Time &_elapsed);
  void escapeFSMS();
  
 protected:
  
  ExtString _strCmd;  
  ExtString _answer;  
  char* _answerBuf;  
  char* _answerBufptr;  
  int _answerBufSize;
  bool _badScan;
  ExtString _temporary; 
  
  Thread _scanThread; 
  
  unsigned int _expectedLines; 
  unsigned int _expectedChars; 
  unsigned int _expectedBytes; 
  unsigned int _alreadyRead;  
  unsigned int _expectedRays;  
  unsigned int _parsedLines;  
  ExtString _parsingBits;    
  ExtString _prevTStamp;    
  Raymms _tempRays;        
  Time _tStamp;         
  unsigned int _totRays;    
  unsigned int _totScans;    
  unsigned int _lfcount;    
  unsigned int _readAttempts;  
  unsigned int _remainingScans; 
  
  bool _wait;     
  bool _isContinuos; 
  bool _echoMatch;  
  bool _statusMatch; 
  bool _tStampAcq;  
  bool _checkLine;  
  bool _scanComplete; 
  bool _workDone;
  bool _verboseOn;  
  bool _log;     
  
  Time _readDataTime;  
  Time _waitNextScan;  
  Time _FSMperiod;   
  Time _waitEchoMatch; 
  Timer _waitedScan;  
  Timer _waitedEcho;  
  
  int _waitNextScanTimeusecs;
  
  ofstream _logFile; 
  
  void parse2bytes ( ExtString _currentLine );
  
  void parse3bytes ( ExtString& _currentLine );

  void updateInfo();

  bool buildMultiScan ( ExtString& command );

  bool buildSingleScan ( ExtString& command );
  
  int readData ( ExtString command,ExtString & rbuf,int how );
  
  int readData ( ExtString command,int how );
  
  void changeMotor ( int value );

  void changeBuad ( int value );
  
  SenderUDP* _sender;
  bool _sendScan;
  char _sendingBuffer[10000];
  int _printedBuffer;
  
  
 public:
  Timer _FSMtimer;
  
  scanWorkState SWstate;
  FSMSstates _state; 
  Scan _myScan;
  
  bool _canDraw;
  /*       FINE      */
  URG04LXmmPar *_URGpar;   
  URG04LXmmConst _URGConst;  
  URG04LXmmInfo *_URGinfo;  
  URG04LXmmCommand *_cmd;   
  
  URG04LXmm(int argc, const char* argv[]);
  
  URG04LXmm(int argc, const char* argv[], SenderUDP* sender);
  
  ~URG04LXmm();
  
  ResourcePlate getPlate() const {
   return _plate;
  }
  
  void setWaitNextScanTimeusecs(int t){
   _waitNextScanTimeusecs = t;
  }
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  
  void setSender(SenderUDP* sender) {
   _sender = sender;
  }
  
  void setWait()
  {
   _wait=true;
  }
  
  void setIsContinuos()
  {
   _isContinuos=true;
  }
  
  void setEchoMatch()
  {
   _echoMatch=true;
  }

  void setStatusMatch()
  {
   _statusMatch=true;
  }

  void setTStampAcq()
  {
   _tStampAcq=true;
  }

  void setScanComplete()
  {
   _scanComplete=true;
  }

  void setCheckLine()
  {
   _checkLine=true;
  }
  
  void setWorkDone()
  {
   _workDone=true;
  }
  
  void setVerboseOn()
  {
   _cmd->setVerboseOn();
   _verboseOn=true;
  }
  
  void setLog()
  {
   _logFile.open("logFile.txt",ios_base::app);
   _cmd->setLog();
   _log=true;
  }
  
  void unsetWait()
  {
   _wait=false;
  }
  
  void unsetIsContinuos()
  {
   _isContinuos=false;
  }
  
  void unsetEchoMatch()
  {
   _echoMatch=false;
  }

  void unsetStatusMatch()
  {
   _statusMatch=false;
  }

  void unsetTStampAcq()
  {
   _tStampAcq=false;
  }

  void unsetScanComplete()
  {
   _scanComplete=false;
  }

  void unsetCheckLine()
  {
   _checkLine=false;
  }
  
  void unsetWorkDone()
  {
   _workDone=false;
  }
  
  void unsetVerboseOn()
  {
   _cmd->unsetVerboseOn();
   _verboseOn=false;
  }
  
  void unsetLog()
  {
   _cmd->unsetLog();
   _log=false;
  }
  
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/

  int totRays()
  {
   return _totRays;
  }
  
  int totScans()
  {
   return _totScans;
  }
  
  bool wait()
  {
   return _wait;
  }
  
  bool isContinuos()
  {
   return _isContinuos;
  }
  
  bool echoMatch()
  {
   return _echoMatch;
  }

  bool statusMatch()
  {
   return _statusMatch;
  }

  bool tStampAcq()
  {
   return _tStampAcq;
  }

  bool scanComplete()
  {
   return _scanComplete;
  }
  
  bool checkLine()
  {
   return _checkLine;
  }
  
  bool workDone()
  {
   return _workDone;
  }
  
  bool verboseOn()
  {
   return _verboseOn;
  }
  
  bool log()
  {
   return _log;
  }
  
  bool updatedScan ( Scan& s );
  
//   bool storedScan ( Scanmm& s,Time timeout );

/***************************************************************************************************
*********************************        MANDATORY METHODS         *********************************
***************************************************************************************************/

  Scan _singleScan();
  
//   void startSingle();
  
  void mySingleScan()
  {
   _state=sendSingle;
   _isContinuos=false;
  };

  void _startScan ( unsigned int num );

  void _stopScan();
  
  void defFSM ();

};


 /* @} */
 
};// end namespace MipResources{


#endif