Odometry2D.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 __ODOMETRY2D_H_
#define __ODOMETRY2D_H_

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

#include <stdio.h>
#include <math.h>
#include <vector>
#include <iostream>
#include <string>
#include <sstream>
#include <sys/time.h>
#include <sys/select.h>
#include <assert.h>

#ifdef FOUND_BOOST_SER
 #include <boost/serialization/vector.hpp>
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
#endif

#include <Types.h>

#include <SE2.h>

using namespace std;



class Odometry2D{
 private:
  Decimal _velX;
  Decimal _velY;
  Decimal _omegaZ;
 public:
  
// /// \brief Default constructor
// inline Odometry2D(){velX = -1.0; velY = -1.0; omegaZ = -1.0;};
  
  inline Odometry2D(Decimal xVel=0.0, Decimal yVel=0.0, Decimal zOmega=0.0){ _velX = xVel; _velY = yVel; _omegaZ = zOmega;}
  
  inline Odometry2D(Position A, Decimal zOmega=0.0){ _velX = A.x(); _velY = A.y(); _omegaZ = zOmega;}
  
  inline Odometry2D(const Odometry2D &A){_velX = A._velX; _velY = A._velY; _omegaZ = A._omegaZ;}
  
  Odometry2D& operator=(const Odometry2D &A){ _velX = A._velX; _velY = A._velY; _omegaZ = A._omegaZ; return *this;}
  
  Odometry2D& operator+=(const Odometry2D& A) { _velX += A._velX; _velY += A._velY; _omegaZ += A._omegaZ; return *this;}
  
  const Odometry2D operator+(const Odometry2D &other) const{ return Odometry2D(*this) += other;}
  
  Odometry2D& operator*=(const Odometry2D& A) { _velX *= A._velX; _velY *= A._velY; _omegaZ *= A._omegaZ; return *this;}
  
  Odometry2D& operator*=(const Decimal& scalar) { _velX = _velX*scalar; _velY = _velY*scalar; _omegaZ = _omegaZ*scalar; return (*this);}
  
  const Odometry2D operator*(const Odometry2D &other) const{ return Odometry2D(*this) *= other;};
  
  Odometry2D operator*(const Decimal& scalar) const {return Odometry2D(*this) *= scalar;}
//  Odometry2D operator*(const Decimal& scalar) const {Odometry2D ris(_velX*scalar, _velY*scalar, _omegaZ*scalar); return ris;}
  
  Odometry2D& operator-=(const Odometry2D& A) {_velX -= A._velX; _velY -= A._velY; _omegaZ -= A._omegaZ; return *this;}
  
  const Odometry2D operator-(const Odometry2D &other) const {return Odometry2D(*this) -= other;}
  
  Odometry2D& operator/=(const Odometry2D& A) {_velX /= A._velX; _velY /= A._velY; _omegaZ /= A._omegaZ; return *this;}
  
  Odometry2D& operator/=(const Decimal& scalar) { _velX = _velX/scalar; _velY = _velY/scalar; _omegaZ = _omegaZ/scalar; return (*this);}
  
  const Odometry2D operator/(const Odometry2D &other) const {return Odometry2D(*this) /= other;}
  
  Odometry2D operator/(const Decimal& scalar) const {return Odometry2D(*this) /= scalar;}
//  Odometry2D operator/(const Decimal& scalar) const {Odometry2D ris(_velX/scalar, _velY/scalar, _omegaZ/scalar); return ris;}
  
  bool operator==(const Odometry2D &other) const { if( (_velX == other._velX) && (_velY == other._velY) && (_omegaZ == other._omegaZ) ) return true; else return false;}
  
  bool operator!=(const Odometry2D &other) const {return !(*this == other);}
  
  inline Decimal getVelX(){ return _velX;}
  
  inline Decimal getVelY(){ return _velY;}
  
  inline Decimal getOmegaZ(){ return _omegaZ;}
  
  inline void setVelX(Decimal xVel){ _velX = xVel;}
  
  inline void setVelY(Decimal yVel){ _velY = yVel;}
  
  inline void setOmegaZ(Decimal zOmega){ _omegaZ = zOmega;}
  
  string print(){
   stringstream ss;
   ss << "velX: " << _velX << " velY: " << _velY << " omegaZ: " << _omegaZ;
   return ss.str();
  }
};
 





#endif



/* @} */