Odometry.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 __ODOMETRY_H
#define __ODOMETRY_H

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

#include "TrajControl.h"
//TODO cancellare le define
#define E_d 0.1
#define E_b 0.1
#define wheel_diameter 0.1
#define axle_length 1.0

// ///\class DiffDriveOdometerPar
// ///\brief Contains the parameters needed to compute the odometric update equations of a differential drive.
// ///\author Antonio Franchi
// ///\todo 
// class DiffDriveOdometerPar{
//  private:
//   Decimal _leftWheelDiam;   ///< Left-wheel diameter.
//   Decimal _rightWheelDiam;  ///< Right-wheel diameter.
//   Decimal _axelTrack;     ///< Axel track.
//   Decimal _encoderResol;   ///< Encoder resolution.
//   
//   Decimal _leftRatio;  ///< Left-wheel conversion-ratio between pulses and linear movement of the wheel.
//   Decimal _rightRatio; ///< Right-wheel conversion-ratio between pulses and linear movement of the wheel.
//  public:
//   /// \brief Constructor.
//   /// \param ld Left-wheel diameter (m).
//   /// \param rd Right-wheel diameter (m).
//   /// \param wb Axel track (m).
//   /// \param er Encoder resolution, number of pulses per radians (pulses/rad).
//   DiffDriveOdometerPar(Decimal ld=1.0,Decimal rd=1.0,Decimal wb=1.0,Decimal er=1.0);
//   /// \brief Copy constructor.
//    DiffDriveOdometerPar(const DiffDriveOdometerPar &p);
//   /// \brief Assigment operator =.
//   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;
//    }
//    return *this;
//   }
//   
//   /// \brief Gets Left-wheel diameter (m).
//   Decimal getLeftWheelDiam();
//   /// \brief Gets Right-wheel diameter (m).
//   Decimal getRightWheelDiam();
//   /// \brief Gets Axel track (m).
//   Decimal getAxelTrack();
//   /// \brief Gets Encoder resolution, number of pulses per radians (pulses/rad).
//   Decimal getEncoderResol();
//   /// \brief Gets Left-wheel conversion-ratio between pulses and linear movement of the wheel.
//   Decimal getLeftRatio();
//   /// \brief Gets Right-wheel conversion-ratio between pulses and linear movement of the wheel.
//   Decimal getRightRatio();
//   /// \brief Prints the parameters.
//   string print();
// };
// 
// 
// ///\class DiffDriveEncoderReading
// ///\brief Contains the readings of the encoders of a differential drive.
// ///\author Antonio Franchi
// ///\todo Public or private variables?
// class DiffDriveEncoderReading{
//  private:
//  public:
//   long int leftPulses; ///< Left-wheel/motor encoder pulses.
//   long int rightPulses; ///< Right-wheel/motor encoder pulses.
//   Time leftTime;    ///< Time in which leftPulses was aquired.
//   Time rightTime;    ///< Time in which rightPulses was aquired.
// 
//   /// \brief Constructor.
//   /// \param lp Left-wheel/motor encoder pulses.
//   /// \param rp Right-wheel/motor encoder pulses.
//   /// \param lt Time in which the left-wheel/motor encoder pulses is aquired.
//   /// \param lt Time in which the right-wheel/motor encoder pulses is aquired.
//   DiffDriveEncoderReading(long int lp=0,long int rp=0,Time lt=Time(0.0),Time rt=Time(0.0));
//   /// \brief Copy constructor.
//   DiffDriveEncoderReading(const DiffDriveEncoderReading &p);
//   /// \brief Assigment operator =.
//   DiffDriveEncoderReading& operator=(const DiffDriveEncoderReading& p){
//    if (this != &p){
//     leftPulses = p.leftPulses;
//     rightPulses = p.rightPulses;
//     leftTime  = p.leftTime;
//     rightTime  = p.rightTime;
//    }
//    return *this;
//   }
//   /// \brief Prints the readings.
//   string print();
// };
//  
//  
// ///\class DiffDriveOdometerVar
// ///\brief Contains the variables needed to compute the odometric update equations of a differential drive.
// ///\author Antonio Franchi
// ///\todo Public or private variables?
// class DiffDriveOdometerVar{
//  private:
//  public:
//   Pose pose; ///< Current Pose, x,y,theta.
//   DiffDriveEncoderReading encodRead; ///< Readings (pulses,times) of the encoders of the wheels.
// 
//   /// \brief default constructor.
// //   DiffDriveOdometerVar(Pose p, long int lp,long int rp,Time lt,Time rt);
//   /// \brief Constructor.
//   /// \param p  Initial Pose of the unicycle: x,y,theta.
//   /// \param lp Initial left-wheel/motor encoder pulses.
//   /// \param rp Initial right-wheel/motor encoder pulses.
//   /// \param lt Time in which the initial left-wheel/motor encoder pulses was aquired.
//   /// \param lt Time in which the initial right-wheel/motor encoder pulses was aquired.
//   DiffDriveOdometerVar(Pose p=Pose(),DiffDriveEncoderReading r=DiffDriveEncoderReading());
// //   DiffDriveOdometerVar(Pose p, long int lp,long int rp,Time lt,Time rt);
// 
//   /// \brief Copy constructor.
//   DiffDriveOdometerVar(const DiffDriveOdometerVar &p);
//   /// \brief Assigment operator =.
//   DiffDriveOdometerVar& operator=(const DiffDriveOdometerVar& p){
//    if (this != &p){
//     pose    = p.pose;
//     encodRead  = p.encodRead;
//    }
//    return *this;
//   }
//    /// \brief Prints the variables.
//    string print();
// };
// 
// 
// ///\class DiffDriveOdometerIn
// ///\brief Contains the iputs needed to compute the odometric update equations of a differential drive.
// ///\author Antonio Franchi
// ///\todo
// typedef DiffDriveEncoderReading DiffDriveOdometerIn;
// 
// ///\class DiffDriveOdometerOut
// ///\brief Contains the outputs returned by the odometric update equations of a differential drive.
// ///\author Antonio Franchi
// ///\todo Public or private variables?
// class DiffDriveOdometerOut{
//  private:
//  public:
//   Pose  pose;   ///< Pose, x,y,theta.
//   Decimal speed;  ///< Current linear speed (m/sec).
//   Decimal turnrate; ///< Current angular velocity (rad/sec).
// 
//   /// \brief Constructor.
//   /// \param p New computed pose of the differntial drive: x,y,theta.
//   /// \param s New computed linear speed.
//   /// \param t New computed angular velocity.
//   DiffDriveOdometerOut(Pose p,Decimal s,Decimal t);
//   /// \brief Copy constructor.
//   DiffDriveOdometerOut(const DiffDriveOdometerOut &p);
//   /// \brief Assigment operator =.
//   DiffDriveOdometerOut& operator=(const DiffDriveOdometerOut& p){
//    if (this != &p){
//     pose   = p.pose;
//     speed   = p.speed;
//     turnrate = p.turnrate;
//    }
//    return *this;
//   }
//    /// \brief Prints the output.
//   string print();
// };
// 
// 
// ///\class DiffDriveOdometer
// ///\brief Algorithm which may be used to update the state of a differential drive. It make use of the odometric equations.
// ///\author Antonio Franchi
// ///\todo Add logging.
// class DiffDriveOdometer{
//  private:
//   DiffDriveOdometerPar par;
//   DiffDriveOdometerVar var;
//  public:
//   /// \brief Constructor.
//   /// \param p Parameters.
//   /// \param v Initial variables (state).
//   DiffDriveOdometer(const DiffDriveOdometerPar& p,const DiffDriveOdometerVar& v);
//   
//   /// \brief Reset the pose to a certain value.
//   /// \param p New pose.
//   void poseReset(Pose p);
//   
//   /// \brief Reset the variables of the algorithm to a certain value.
//   /// \param p New variables.
//   void varReset(DiffDriveOdometerVar v);
//   
//   /// \brief Fundamental step of the algorithm.
//   /// \param in Inputs of the step (new pulses and times).
//   /// \return Output of the step (new pose, new linear speed, new angular speed).
//   DiffDriveOdometerOut step(DiffDriveOdometerIn in);
// };


class Odometry{
 private:
  bool first;//TODO si può omettere se inizializzo tutto nel costruttore?
//   long int N_LeftW_prec,N_RightW_prec;//Input di update
  long int _leastLeftRead, _leastRightRead;
//    Timer _encTimer;
  Time _leastLeftTime,_leastRightTime;
 public:
  
  bool updateOdometry(long int currLeftRead, long int currRightRead, long int encRes, Time currLeftTime, Time currRightTime,TrajectoryState &trajStatus){
   Decimal dT,dTR,dTL;  
   Decimal vr, vl;
   long   N_LeftW, N_RightW;
   Decimal vel_tick_r,vel_tick_l;
//    Time currLeftTime,currRightTime;
   
   static Decimal Deltang=(2.0*M_PI/encRes);
   if(first){//TODO verifica
    _leastLeftRead  = 0;//NOTE origine della lettura?
    _leastRightRead = 0;//NOTE origine della lettura?
    _leastLeftTime=Time(0,0);
    _leastRightTime=Time(0,0);
//     _encTimer.reset();
    first = false;
   }
  
//   wheelCountersL = getLeftEncoder();//NOTE nuova della lettura?
//   double ltime   = leggiTempo3(tvl);
//   currLeftTime=_encTimer.get();
//   wheelCountersR = getRightEncoder();//NOTE nuova della lettura?
// //   double rtime   = leggiTempo3(tvr);
//   currRightTime=_encTimer.get();
  
  int tent = 0;
  int maxTent =2;
  while( ( (currLeftRead - _leastLeftRead > 200) || (currLeftRead - _leastLeftRead < -200) ) && (tent<maxTent) ){/*provo a filtrare letture fallaci*/
//    wheelCountersL = getLeftEncoder();
//    ltime          = leggiTempo3(tvl);
   //TODO aggiornare comunque least readings?qui wheelCountersL
   tent++;
   return false;
  }
  tent=0;
  N_LeftW = currLeftRead - _leastLeftRead;
  while( ( (currRightRead - _leastRightRead > 200)|| (currRightRead - _leastRightRead < -200) )  && (tent<maxTent) ){/*provo a filtrare letture fallaci*/
//    wheelCountersR = getRightEncoder();
//    rtime          = leggiTempo3(tvr);
   //TODO aggiornare comunque least readings?qui wheelCountersR
   tent++;
   return false;
  }
  N_RightW = currRightRead - _leastRightRead;
//  if(wheelCountersL > 2765){
//    wheelCountersL  = getLeftEncoder();
//    ltime = leggiTempo3(tvl);
//  }
//  N_LeftW=wheelCountersL-N_LeftW_prec;
  //  /*!filtraggio per evitare letture incoerenti (maggiori della risoluzione max dell'encoder)*/
//  if(wheelCountersR > 2765){ 
//   wheelCountersR  = getRightEncoder();
//   rtime = leggiTempo3(tvr);
//  }
//  N_RightW=wheelCountersR-N_RightW_prec;
  Time dtLeft=currLeftTime-_leastLeftTime;
  Time dtRight=currRightTime-_leastRightTime;
  dTL = dtLeft.dCast();
  dTR =dtRight.dCast();
  Time dTime=(Time)(dtRight+dtLeft)*0.5;
  dT=dTime.dCast();
 
 //Calcolo di vl, vr, v & omega
  vel_tick_l = rint(N_LeftW/dTL);  //pulse/s
  vel_tick_r = rint(N_RightW/dTR); //pulse/s

  _leastLeftTime = currLeftTime;
  _leastRightTime = currRightTime;
  vl=(2.0/ (E_d+1.0))          * (N_LeftW  * Deltang * (wheel_diameter/2.0)) / dTL;  // m/s
  vr=(2.0/ ( (1.0/E_d) +1.0) ) * (N_RightW * Deltang * (wheel_diameter/2.0)) / dTR; // m/s
  
  trajStatus.setDtWheel((Position)(dTR,dTL));
//   MState.dTL = dTL;
//   MState.dTR = dTR;
  trajStatus.setWheelVel((Position)(vr,vl));
//   MState.vl = vl;
//   MState.vr = vr;
  trajStatus.setStatusVel(VelVec(0.5*(vr+vl),(vr-vl)/(E_b*axle_length),dTime));
//   MState.V  = 0.5*(vr+vl);
//   MState.W  = (vr-vl)/(E_b*axle_length);
//   dT        = 0.5*(dTR + dTL);
 
  //Integrazione Runge-Kutta del II ordine
  Angle theta=trajStatus.statusPose().ori();
  Decimal V=trajStatus.statusVel().linVel();
  Decimal W=trajStatus.statusVel().angVel();
//   Pose dPose=Pose (cos(theta.dCast() + W*dT*0.5 )*V*dT,sin(theta.dCast() + W*dT*0.5 )*V*dT,(Angle)(normalize(theta.dCast()+(W*dT))));
  Pose dPose=Pose (cos(theta.dCast2Pi() + W*dT*0.5 )*V*dT,sin(theta.dCast2Pi() + W*dT*0.5 )*V*dT,(Angle)(theta.dCast2Pi()+(W*dT)));
  trajStatus.setStatusPose(trajStatus.statusPose()+dPose);
    
//   MState.x +=  cos( MState.theta + MState.W*dT*0.5 )*MState.V*dT.dCast();
//   MState.y +=  sin( MState.theta + MState.W*dT*0.5 )*MState.V*dT.dCast();
//   MState.theta = normalizza( MState.theta + MState.W * dT);
         
  //Aggiornamento dello stato
  _leastLeftRead  = currLeftRead;
  _leastRightRead = currRightRead; 

//   if(log_flag){
//    MLogState.x = MState.x;
//    MLogState.y = MState.y;
//    MLogState.theta = MState.theta;
//    MLogState.leftTicks  = wheelCountersL;
//    MLogState.rightTicks = wheelCountersR;
//    MLogState.leftTv = tvl;
//    MLogState.rightTv = tvr;
//   }
  return true;
 }
};


#endif


/* @} */