URG04LX.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 __URG04LX_H_
#define __URG04LX_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 <socketUDP.h>

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



namespace MipResources{


/* @{ */

// enum scanWorkState{
//  _FIRST_SCAN,
//   _NORMAL_SCAN,
//  _SCAN_WORK_STATE_NUM
// };

// enum FSMSstates{
//  sendSingle,  /**<sending single scan command*/
//  sendMulti,  /**<sending multiple scan command*/
//  searchAnswer, /**<looking for correct answer*/
//  getTime,
//  receive,   /**<answer reading*/
//  storeData,  /**<data storing*/
//  parse,    /**<data parsing*/
//  storeScan,  /**<scan storing*/
//  reset,    /**<resetting after complete scan or "non-fatal" error*/
//  waitEcho,   /**<waiting for correct answer*/
//  waitScan,   /**<waiting for next scan*/
//  recovery,   /**<recovering after a "fatal" error*/
//  escape    /**<exit after exhausting scan request*/
// };

class URG04LXConst{
 private:
  static const Decimal _linRes;             
  static const Decimal _linRangeMin;          
  static const Decimal _linRangeMax2B;        
  static const Decimal _linRangeMax3B;        
  static const Decimal _angRes;            
  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:
  URG04LXConst(){
   _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 Decimal linRes(){
   return _linRes;
  }

  static const Decimal linRangeMin(){
   return _linRangeMin;
  }

  static const Decimal linRangeMax2B(){
   return _linRangeMax2B;
  }

  static const Decimal linRangeMax3B(){
   return _linRangeMax3B;
  }

  static const Decimal 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 URG04LXCommand : public MIPObject{
 private:
  URG04LXConst _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:

  URG04LXCommand(){
   
   _device=_URGConst.dev();
   _fd=::open(_device, O_RDWR|O_SYNC|O_NOCTTY);
   if (_fd==-1){
    fatal("Device not reachable, turn it on or add the option -mobileRobLaser 0");
    setfdBad();
    return;
   }
   unsetfdBad();
   _laserPort=fopen(_device,"r+");
   unsetSCIP2();
   unsetFirstBlock();
   unsetIsFinished();
   unsetVerboseOn();
   unsetLog();
//    _URGConst = new URG04LXConst;
  }

  ~URG04LXCommand(){
   if (_fd!=-1){
    if (log())
     _logFile.close();
    fclose(_laserPort);
   }
//    delete _URGConst;
  }

  string getObjectName() const {
   return "URG04LX";
  }

  void setDevice (const char* device){
   fclose(_laserPort);
   open(device, O_RDWR|O_SYNC|O_NOCTTY);
   _device=device;
   _fd=::open(_device, O_RDWR|O_SYNC|O_NOCTTY);
   _laserPort=fopen(_device,"r+");
  }
  
  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 URG04LXInfo{
 private:

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

  URG04LXInfo();

/***************************************************************************************************
***********************************        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 URG04LXPar : public RangeSensPar{
 private:
  URG04LXConst _URGConst; 
  int _enc;      
  int _firstStep;   
  int _lastStep;   
  int _cluster;    
  int _scanInterval; 
  int _scanNum;    
  int _motorSpeed;  
  int _baudRate;   
  Time _maxCmdTime; 

  void setParam();

  public:
  URG04LXPar();

  ~URG04LXPar();

/***************************************************************************************************
***********************************        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 URG04LX : public RangeSens{
 private:
  static const ResourcePlate _plate = RS_URG_RES; 
 protected:

  ExtString _strCmd;  
  ExtString _answer;  
  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;    
  Rays _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;  
  //Timer _scanTime;   /**<length of a scan*/

  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);

  void changeMotor(int value);

  void changeBuad(int value);
  
 public:
  Timer _FSMtimer;
  
  scanWorkState SWstate;
  FSMSstates _state; 
  Scan _myScan;
  
  bool _canDraw;
  /*       FINE      */
  URG04LXPar *_URGpar;   
  URG04LXConst _URGConst; 
  URG04LXInfo *_URGinfo;  
  URG04LXCommand *_cmd;   

  URG04LX(int argc, const char* argv[]);
  
  ~URG04LX();
  
  ResourcePlate getPlate() const{
   return _plate;
  }
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  
  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(Scan& 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