KorebotROS.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 __KOREBOT_ROS_H_
#define __KOREBOT_ROS_H_

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <iostream>
#include <fstream>

#include <baselib.h>
#include "MotionModule.h"
#include <includeROS.h>

namespace MipResources{
 /* @{ */
 

class KorebotROSPar : public DiffDrivePar{
 private:
  // Robot name
  string     _korebotName;
  
  // Maximum wheel turn rate
  Decimal     _maxWheelTurnRate;
  
  // Diameter of the left wheel
  Decimal     _leftDiam;
  
  // Diameter of the right wheel
  Decimal     _rightDiam;
  
  // Separation between the wheels
  Decimal     _axelTrack;
  
  // Encoder resolution
  Decimal     _encoderResol;
  
  // Pointer to left joint
  GAZEBO_JOINT_PTR _leftJoint;
  
  // Pointer to right joint
  GAZEBO_JOINT_PTR _rightJoint;
  
  // Left ratio, defined as (M_PI*leftDiameter)/(encoderResolution)
  Decimal     _leftRatio;
  
  // Right ratio, defined as (M_PI*rightDiameter)/(encoderResolution)
  Decimal     _rightRatio;
  
  // Integration method for odometer
  string     _intOdoMeth;
  
 public:
  KorebotROSPar();
  
  KorebotROSPar(const string &kn, const Decimal &mwtr, const Decimal &ld, const Decimal &rd, const Decimal &at, const Decimal &er, GAZEBO_JOINT_PTR lj, GAZEBO_JOINT_PTR rj, const string &intOdoMeth);
  
  KorebotROSPar(GAZEBO_JOINT_PTR lj, GAZEBO_JOINT_PTR rj, const string &intOdoMeth);
  
  KorebotROSPar(const string &kn, const Decimal &mwtr, const Decimal &ld, const Decimal &rd, const Decimal &at, const Decimal &er, const string &intOdoMeth);
  
  ~KorebotROSPar(){
  }
  
  string  _providedName();
  
  Decimal _providedMaxWheelTurnRate(void);
  
  Decimal _providedLeftDiam(void);
  
  Decimal _providedRightDiam(void);
  
  Decimal _providedAxelTrack(void);
  
  Decimal _providedEncoderResol(void);
  
  GAZEBO_JOINT_PTR getLeftJoint();
  
  GAZEBO_JOINT_PTR getRightJoint();
  
  Decimal getLeftRatio() const;
  
  Decimal getRightRatio() const;
  
  string getIntOdoMeth() const;
  
  string print();
};

class KorebotROSVar : public DiffDriveVar{
 private:
  
 public:
  KorebotROSVar();
  
  virtual string getName(void){
   return "MotionKorebotROS";
  }
};


class KorebotROS : protected DiffDrive{
 private:
  static const ResourcePlate _plate = MM_KORROS_RES; 
  
  // Time of the last encoder measurement
  GAZEBO_COMMON_NAMESPACE::Time* _lastTime;
  Decimal    _odomPose[3];
  Decimal    _odomVel[3];
  
  // Pulses from encoders
  long int   _lastLeftPulses;
  long int   _lastRightPulses;
  
  // Actual velocities on left and right wheels 
  Decimal    _actLeftWheelSpeed;
  Decimal    _actRightWheelSpeed;
  
  // Previous computed velocities: needed for Tustin Integration
  Decimal    _prevLeftWheelSpeed;
  Decimal    _prevRightWheelSpeed;
  
  // Actual linear and angular speed
  Decimal    _linearSpeed;
  Decimal    _angularSpeed;
  
  // Check if it is the first step or not
  bool      _firstStep;
  
  GAZEBO_COMMON_NAMESPACE::Time _actIntTime;
  GAZEBO_COMMON_NAMESPACE::Time _lastIntTime;
  Decimal    _elapsedTimeROS;
  
  // Private methods:
  void _updateIntTime();
 protected:
  /* PROMEMO from more abstract classes */
  /* UnicyclePar *_par; */
  /* UnicycleVar *_var; */
  /* DiffDrivePar *_ddPar; */
  /* DiffDriveVar *_ddVar; */
  KorebotROSPar *_korePar; 
  KorebotROSVar *_koreVar; 
  // The variables count the left and right pulses emulating the incremental encoders
  long int _leftPulses;
  long int _rightPulses;
  
 public:
  KorebotROS(int argc, const char* argv[],KorebotROSPar* krp);
  
  ~KorebotROS();
  
  ResourcePlate getPlate() const {
   return _plate;
  }

  void setWheelCommands(Decimal leftAngVel, Decimal rightAngVel);
  
  DiffDriveEncoderReading getEncoders();
  
  void resetEncoders();
  
  bool setStateKorebotROS(MotionModuleState correctedState, bool setPastTimeStamp=false, Time pastTimeStamp=Time());
  
  void _localizationStepKorebotROS();
  
  MotionModuleTState _providedLocStepKorebotROS();
  
  void _initOdometerKorebotROS();
  
  void _computeVelocities(DiffDriveEncoderReading &currReading);
  
  void _intVelocities();
  
  void _setWheelCommandsKorebotROS();
  
  void _providedLocSetKorebotROS(MotionModuleState state);
  
  MotionModuleTState getStateKorebotROS();
  
  MotionModuleTState getState(Time ts);
  
  bool setStateKorebotROS(MotionModuleState correctedState, MotionModuleState &finalState, bool setPastTimeStamp=false, Time pastTimeStamp=Time());
  
  void setCommandsKorebotROS(const Decimal &drive, const Decimal &turnrate, bool scaled=true);
  
  void setCommandsKorebotROS(const Decimal &drive, const Decimal &turnrate, MotionModuleTState &finalTState, bool scaled=true);
  
  void setCommandsAndTStateKorebotROS(MotionModuleState correctedState, const Decimal &drive, const Decimal &turnrate, MotionModuleTState &finalTState, bool scaled=true, bool setPastTimeStamp=false, Time pastTimeStamp=Time());
  
  void _providedSetCommandsKorebotROS(const Decimal &drive, const Decimal &turnrate);
  
};
 
 
 /* @} */
 
};// end namespace MipResources

#endif