DiffDriveOdometer.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 __DIFF_DRIVE_ODOMETER_H_
#define __DIFF_DRIVE_ODOMETER_H_

#include <baselib.h>

namespace MipAlgorithms{
 
// enum OdomIntegrMethods{
//  ODO_INT_METH_EXACT,
//  ODO_INT_METH_RUNKUT2,
//  ODO_INT_METH_NUM
// };

class DiffDriveOdometerPar : public MIPObject{
 private:
  Decimal _leftWheelDiam;   
  Decimal _rightWheelDiam;  
  Decimal _axelTrack;     
  Decimal _encoderResol;   
  
  long int _maxPulseRate; 
  
  Decimal _leftRatio;  
  Decimal _rightRatio; 
  
  OdomIntegrMethods _integrMethod;
  
  string getObjectName() const {
   return string("DiffDriveOdometer");
  }
  
 public:
  DiffDriveOdometerPar(Decimal ld=1.0,Decimal rd=1.0,Decimal wb=1.0,Decimal er=1.0,Decimal maxWheelTurnRate=100000.0, string iM="runge-kutta2");
  DiffDriveOdometerPar(const DiffDriveOdometerPar &p);
  DiffDriveOdometerPar& operator=(const DiffDriveOdometerPar& p){
   if (this != &p){
    _leftWheelDiam = p._leftWheelDiam;
    _rightWheelDiam = p._rightWheelDiam;
    _axelTrack   = p._axelTrack;
    _encoderResol  = p._encoderResol;
    _leftRatio   = p._leftRatio;
    _rightRatio   = p._rightRatio;
    _maxPulseRate  = p._maxPulseRate;
    _integrMethod  = p._integrMethod;
   }
   return *this;
  }
  
  Decimal getLeftWheelDiam();
  Decimal getRightWheelDiam();
  Decimal getAxelTrack();
  Decimal getEncoderResol();
  Decimal getLeftRatio();
  Decimal getRightRatio();
  long int getMaxPulseRate();
  OdomIntegrMethods getIntegrMethod();
  string print();
};

 
class DiffDriveOdometerVar{
 public:
  Pose pose; 
  DiffDriveEncoderReading encodRead; 
  long unsigned int step;

//   DiffDriveOdometerVar(Pose p, long int lp,long int rp,Time lt,Time rt);
  DiffDriveOdometerVar(Pose p=Pose(),DiffDriveEncoderReading r=DiffDriveEncoderReading());
//   DiffDriveOdometerVar(Pose p, long int lp,long int rp,Time lt,Time rt);

  DiffDriveOdometerVar(const DiffDriveOdometerVar &p);
  DiffDriveOdometerVar& operator=(const DiffDriveOdometerVar& p){
   if (this != &p){
    pose    = p.pose;
    encodRead  = p.encodRead;
    step    = p.step;
   }
   return *this;
  }
  string print();
};



class DiffDriveOdometer : public MIPObject{
 private:
  DiffDriveOdometerPar _par;
  DiffDriveOdometerVar _var;
  DiffDriveOdometerVar _prevVar;
  fstream  _logFile;
  
  string getObjectName() const {
   return string("DiffDriveOdometer");
  }
  
 public:
  DiffDriveOdometer(const DiffDriveOdometerPar& p,const DiffDriveOdometerVar& v);
  
 ~DiffDriveOdometer(){
  _logFile.close();
 }
  
// /// \brief Reset the pose to a certain value.
// /// \param p New pose.
//   void poseReset(Pose p);
  
  void reset(Pose& pose, DiffDriveEncoderReading& encodRead);
  
//   void varReset(DiffDriveOdometerVar v);
  
  bool step(DiffDriveEncoderReading in,MotionModuleTState &out);
};
} // End of namespace

#endif