MotionModule.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_H_
#define __MOTION_MODULE_H_

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

#include <stdlib.h>
#include <string>
#include <sstream>
#include <vector>
#include <assert.h>
#include <list>

#include <baselib.h>
#include <Resource.h>

#include <DiffDriveOdometer/DiffDriveOdometer.h>

namespace MipResources{

 /* @{ */

class MotionModuleOptions : public Options {
 public:
  PoseOption  *initialPose;
  StringOption *odomIntegrMethod;
  
  MotionModuleOptions();
  OptionGroupsType getGroup();

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

using namespace MipBaselib;

class MotionModulePar{
 protected:  
  virtual Decimal _providedMaxDrive(void)=0;
  virtual Decimal _providedMaxTurnRate(void)=0;
  virtual string  _providedName()=0;
 public:
  MotionModulePar (){};

  Decimal maxDrive(void);
  Decimal maxTurnRate(void);
  string name();
  
  virtual string print();
};


class MotionModule : public Resource{
 protected:
//  list<MotionModuleTState> _varLog; ///< Log of the state
  MotionModuleTStateLog _varLog;
//   int _logSize;
 private:
  void _checkAllocation();

  void _localizationStep();
  
  
 protected:
  MotionModulePar *_par;
  MotionModuleTState *_var;
  MotionModuleOptions _options;
  
  virtual MotionModuleTState _providedLocStep()=0;
  virtual void _providedLocSet(MotionModuleState state)=0;
  virtual void _providedSetCommands(Decimal drive, Decimal turnrate)=0;
 public:
  MotionModule(int argc, const char* argv[]);
  
  virtual ResourcePlate getPlate() const =0;
  
  MotionModuleTState getState();
  
  virtual MotionModuleTState getState(Time ts);
  
  bool setState(MotionModuleState state, bool setPastTimeStamp=false, Time pastTimeStamp=Time());
  
  bool setState(MotionModuleState correctedState, MotionModuleState &finalState, bool setPastTimeStamp=false, Time pastTimeStamp=Time());
  
  void setCommands(Decimal drive, Decimal turnrate, bool scaled=true);

  void setCommands(Decimal drive, Decimal turnrate, MotionModuleTState &finalTState, bool scaled=true);
  
  void setCommandsAndTState(MotionModuleState correctedState, Decimal drive, Decimal turnrate, MotionModuleTState &finalTState, bool scaled=true, bool setPastTimeStamp=false, Time pastTimeStamp=Time());

  virtual MotionModulePar* getPar(void){
   return _par;
  }
  
  void getCurvature(Decimal &c, bool &infinity);
  
};

class UnicyclePar : public MotionModulePar{};

class UnicycleVar : public MotionModuleTState{};

class Unicycle : public MotionModule{
 public:
  
  Unicycle(int argc, const char* argv[]) : MotionModule(argc,argv){};
  
  virtual ResourcePlate getPlate() const =0;
};

class DiffDrivePar : public MotionModulePar{
 protected:
  Decimal _providedMaxDrive(void);
  Decimal _providedMaxTurnRate(void);
  virtual string  _providedName()=0;
  virtual Decimal _providedMaxWheelTurnRate(void)=0;
  virtual Decimal _providedLeftDiam(void)=0;
  virtual Decimal _providedRightDiam(void)=0;
  virtual Decimal _providedAxelTrack(void)=0;
  virtual Decimal _providedEncoderResol(void)=0;
 public:
  DiffDrivePar (){};
  
  Decimal maxWheelTurnRate(void);
  
  Decimal leftDiam(void);
  
  Decimal rightDiam(void);
  
  Decimal averageDiam(void);
  
  Decimal axelTrack(void);
  
  Decimal encoderResol(void);
  
  string print();
};

class DiffDriveVar : public UnicycleVar{};


class DiffDrive : public Unicycle{
 protected:
  MipAlgorithms::DiffDriveOdometer* _odometer;
  DiffDriveEncoderReading _prevEncoder;
  DiffDriveEncoderReading _currEncoder;
 private:
  void _initOdometer();
  void _checkAllocation();
 protected:
  /* PROMEMO from more abstract class */
  /* MotionModulePar *_par; */
  /* MotionModuleTState *_var; */
  DiffDrivePar *_ddPar;
  DiffDriveVar *_ddVar;
  
  MotionModuleTState _providedLocStep();
  void _providedLocSet(MotionModuleState state);
  void _providedSetCommands(Decimal drive, Decimal turnrate);

 public:
  DiffDrive(int argc, const char* argv[]);
  
  ~DiffDrive();
  
  virtual ResourcePlate getPlate() const =0;
  
  virtual void setWheelCommands(Decimal leftAngVel, Decimal rightAngVel)=0;
  
  virtual DiffDriveEncoderReading getEncoders()=0;
  
  DiffDrivePar* getPar(){
   return _ddPar;
  }
};

 /* @} */
 
};// end namespace MipResources{


#endif