MotionModuleState.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 __MOTION_MODULE_STATE_H_
#define __MOTION_MODULE_STATE_H_

#include <SE2.h>
#include <LogTrace.h>
#include <list>

namespace MipBaselib{

enum TimestampLocation{
 TSPOS_IS_INSIDE,
 TSPOS_IS_BEFORE,
 TSPOS_IS_AFTER,
 TSPOS_NUM
};

class MotionModuleState : public MIPObject{
 
 public:
  Pose  pose;  
  Decimal linVel; 
  Decimal angVel; 
  
  MotionModuleState(Pose p=Pose(),Decimal lv=0.0,Decimal av=0.0);
  
  MotionModuleState(const MotionModuleState &v);
  
  MotionModuleState& operator=(const MotionModuleState& v);
  
  MotionModuleState& operator+=(const MotionModuleState& a);
  
  MotionModuleState& operator-=(const MotionModuleState& a);
  
  const MotionModuleState operator+(const MotionModuleState &other) const {
   return MotionModuleState(*this) += other;
  }
  
  const MotionModuleState operator-(const MotionModuleState &other) const {
   return MotionModuleState(*this) -= other;
  }

  string getObjectName() const {
   return "MotionModuleState";
  }
  
  Position velVector(void);

  virtual string print();
};

class MotionModuleTState : public MotionModuleState{
 public:
  Time timeStamp; 
  
  MotionModuleTState(Pose p=Pose(),Decimal lv=0.0,Decimal av=0.0,Time ts=Time());
  
  MotionModuleTState(const MotionModuleTState &v);
  
  MotionModuleTState& operator=(const MotionModuleTState& v){
   if (this != &v){
    pose  = v.pose;
    linVel = v.linVel;
    angVel = v.angVel;
    timeStamp = v.timeStamp;
   }
   return *this;
  }
  string print();
};

class MotionModuleTStateLog : public list <MotionModuleTState>{
 private:
  MotionModuleTState _interpolation(list<MotionModuleTState>::reverse_iterator rit,list<MotionModuleTState>::reverse_iterator ritBefore,Time ts);
 public:
  
//  /// \brief Gets the state in the log (pose, velocities and timestamp) which timestamp is the nearest to the give timestamp.
//  /// \return A flag which is \b true if a state inside the given tollerance is found, \b false otherwise.
//  /// \param[out].&nearestTState The state found, it makes sense only if return value is true.
//  /// \param[in].ts The state to find in the log must have the nearest timestamp to this parameter.
//  /// \param[in].tollerance If the last state in the log is less recent than ts more than tollerance a new getState() is performed.
//  /// \todo to test
//   bool getNearestTState(MotionModuleTState& nearestTState, Time ts,Time tollerance = Time(1,0));
  
  TimestampLocation getTState(MotionModuleTState& outputTState, Time timeStamp);
  
  TimestampLocation correctTState(MotionModuleTState& correctedTState);
  
  string print(){
   int thisSize = this->size();
   stringstream ss;
//    for(int i = 0; i < thisSize; i++){
    ss << this->begin()->pose.print() << " " << this->begin()->timeStamp.dCast() << endl;
    ss << this->rbegin()->pose.print()<< " " << this->rbegin()->timeStamp.dCast() << endl;
//    }
   return ss.str();
  }
//   MotionModuleTStateLog::iterator getLastItemPointer();
};

} // End of namespace
#endif