MobileRob3D.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 __MOB_ROB_3D_H_
#define __MOB_ROB_3D_H_

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

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

#include <CommonOptions.h>

#include <baselib.h>
#include <RangeSens.h>
// #include <RangeSensmm.h>
#include <MotionModule3D.h>
#include <PositionDetector3D.h>
#include <Imu3D.h>

#include <Resource.h>
#include <MIPMatrix.h>
#include <Surface.h>


namespace MipResources{
 /* @{ */

class MobileRob3DOptions : public Options {
 public:
  IntOption* motionModule;
  IntOption* laser;
  IntOption* infrared;
  IntOption* sonar;
  IntOption* gps;
  IntOption* odoLoc;
  IntOption* imu;
  
  MobileRob3DOptions(){

   motionModule= createIntOption("mobileRob3DMotionModule",  "whether (1) or not (0) the motion module is onboard, default 1", 1);
   laser   = createIntOption("mobileRob3DLaser",  "whether (1) or not (0) the laser is onBoard, default 1", 1);
   infrared= createIntOption("mobileRob3DInfrared",  "whether (1) or not (0) the infrared is onboard, default 0", 0);
   sonar   = createIntOption("mobileRob3DSonar",  "whether (1) or not (0) the sonar is onboard, default 0", 0);
   gps     = createIntOption("mobileRob3DGps",  "whether (1) or not (0) the gps is onboard, default 0", 0);
   odoLoc =       createIntOption("mobileRobOdometryLocalization", "whether (1) or not (0) the localization is given by simple odometry", 1);
   imu     = createIntOption("mobileRob3DImu",  "whether (1) or not (0) the imu is onboard, default 0", 0);
   
   updateValues();

  }

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


class MobileRob3DPar{
 private:
  int      _id;     
  Decimal  _lenght; 
  Decimal  _width;  
  Decimal  _height;  
  string  _name;   
  Decimal  _weelDiam;

 protected:
  void setId(int i){
   assert(i>0);
   _id=i;
  }
  void setLength(Decimal r){
   assert(r>0.0);
   _lenght=r;
  }
  void setWidth(Decimal r){
   assert(r>0.0);
   _width=r;
  }
  void setHeight(Decimal r){
   assert(r>0.0);
   _height=r;
  }

  void setName(string n){
   _name=n;
  }
 public:
  MobileRob3DPar (){
   _id  = 0;
   _lenght  = 0.0;
   _width  = 0.0;
   _height  = 0.0;
   _name  = string();
   _weelDiam  = 0.1; //10cm
  }
  
  MobileRob3DPar(const MobileRob3DPar &r){
   _id  = r._id;
   _lenght  = r._lenght;
   _width  = r._width;
   _height  = r._height;
   _name  = r._name;
   _weelDiam  = r._weelDiam;
  }
  
  MobileRob3DPar& operator=(const MobileRob3DPar& r){
   if (this != &r){
    _id  = r._id;
    _lenght  = r._lenght;
    _width  = r._width;
    _height  = r._height;
    _name  = r._name;
    _weelDiam = r._weelDiam;
   }
   return *this;
  }
  int id()
  {
   return _id;
  }
  Decimal lenght()
  {
   return _lenght;
  }
  Decimal width()
  {
   return _width;
  }
  Decimal height()
  {
   return _height;
  }
  string name()
  {
   return _name;
  }
  Decimal weelDiam()
  {
   return _weelDiam;
  }

  string print()
  {
   stringstream s;
   s << "name " << name() << endl
     << "id " << _id << endl 
     << "lenght " << _lenght << endl
     << "width " << _width << endl
     << "height " << _height << endl
     << "height " << _weelDiam << endl;
   return s.str();
  }
};


class MobileRob3DVar{
 protected:
  Pose3D  _pose;
  Pose3D  _goal;
  Position _goalVel;
 
 public:
  MobileRob3DVar (){}
  
  MobileRob3DVar(const MobileRob3DVar &v)
  {
   _goal  = v._goal;
   _goalVel = v._goalVel;
  }
  
  MobileRob3DVar& operator=(const MobileRob3DVar& v)
  {
   if (this != &v){
    _goal  = v._goal;
    _goalVel = v._goalVel;
   }
   return *this;
  }
  
  void setPose(Pose3D p)
  {
   _pose=p;
  }
  
  Pose3D pose()
  {
   return _pose;
  }
  
  void setGoal(Pose3D g)
  {
   _goal=g;
  }
  
  Pose3D goal()
  {
   return _goal;
  }
  
  void setGoalVel(Position gv)
  {
   _goalVel=gv;
  }
  
  Position getGoalVel()
  {
   return _goalVel;
  }
  
  string print()
  {
   stringstream s;
   s << "goal " << _goal.print() << ", goalVel " << _goalVel.print();
   return s.str();
  }
};


class MobileRob3D : public Resource{
 protected:
  MobileRob3DOptions options; 
  MobileRob3DPar  *_par;   
  MobileRob3DVar  *_var;  
  // onboard motion modules
  Unicycle3D    *_unicycle3D; 
  // onboard sensors and actuators
  RangeSens     *_rangeSens; 
//  RangeSensmm     *_rangeSensmm; /**< abstract Robot range sensor */
  //RangeSens     *_infrared; /**< abstract Robot range sensor infrared*/
  //RangeSens     *_sonar; /**< abstract Robot range sensor sonar*/
  PositionDetector3D    *_positionDetector3D; 
  Imu      *_imu;   
  ScanTypes rangeSensorType; 
  bool unicycleActive;
  
  bool rangeSensActive;
  bool rangeSensmmActive;
  bool infraredActive;
  bool sonarActive;
  bool gpsActive;
  bool imuActive;
  
  bool useOdometryLocalization; 
  
 public:
  MobileRob3D(){}
  
  MobileRob3D(int argc, const char* argv[])
  {
          cout << "MobileRob3D constructor starting" << endl;
   unicycleActive  = false;
   
   //rangeSensActive = false;
   //rangeSensmmActive = false;
   //infraredActive = false;
   //sonarActive  = false;
   gpsActive  = false;
   imuActive  = false;
   
   if (options.odoLoc->getValue())
   {
    useOdometryLocalization = true;
   }
   else
   {
    useOdometryLocalization = false;
   }

   cout << "MobileRob3D constructor OK." << endl;
  }
  
  /*virtual TODO logging, anzi il logger è un task */
  
  virtual MobileRob3DPar* robPar()
  {
   return _par;
  }
  
  virtual MobileRob3DVar* robVar()
  {
   return _var;
  }
  
  
  /* ### ### ### Motion Module ### ### ### */
  virtual Unicycle3DPar* unicycle3DPar()
  {
   if (unicycleActive)
    return (Unicycle3DPar*) _unicycle3D->par();
   else
    return NULL;
  }
  
  
  virtual void velCmd(Decimal drive, Decimal turnrate)
  {
   if (unicycleActive)
   {
    _unicycle3D->velCmd(drive, turnrate);
   }
  }
  
  
  virtual void getVel(Decimal& drive, Decimal& turnrate)
  {
   if (unicycleActive)
    _unicycle3D->getVel(drive,turnrate);
  }
  
  
  virtual Pose3D getPose(void)
  {
   cout << "Start getPose" << endl;
   if (unicycleActive)
   {
    //cout << "2" << endl;
    if (useOdometryLocalization)
    {
     //cout << "A" << endl;
     return _unicycle3D->odometry();
    }
    else
    {
     //cout << "A1" << endl;
     return _var->pose();
    }
   }
   else
    return Pose3D(Position3D(0.0,0.0,0.0), Orientation3D(0.0,0.0,0.0));
  }
  
  
  virtual void setPose(Pose3D p)
  {
   if (unicycleActive){
    if (useOdometryLocalization)
     _unicycle3D->setOdometry(p);
    else
     return _var->setPose(p);
   }
  }
  
  
  virtual void getOdometry(MIPMatrix &RobStat, Decimal *ENCtime)
  {
          cout << "MobileRob3D::getOdometry()" << endl;
   _unicycle3D->odometryDiff(RobStat, ENCtime);
   return;
  }

  /* ### ### gps ### ### */
  virtual bool getGPSMeasures(Position3D &pos, Decimal *MeasTime)
  {
      cout << "MobileRob3D::getGPSMeasures()." << "\n";
      return(_positionDetector3D->getGPSMeasure(pos, MeasTime));
  }

  virtual bool getRefGPSMeasures(Position3D &pos, Decimal *MeasTime)
  {
      return(_positionDetector3D->getRefGPSMeasure(pos, MeasTime));
  }

  virtual bool getIMUMeasures(Acceleration3D &acc, Acceleration3D &gyro, Orientation3D &magn, Decimal *MeasTime)
  {
      cout << "MobileRob3D::getIMUMeasures()." << "\n";
      return(_imu->getIMUMeasure(acc, gyro, magn, MeasTime));
  }
  
  /* ### ### MobileRob ### ### */
  void setGoal(Pose3D g)
  {
   _var->setGoal(g);
  }
  
  
  Pose3D goal()
  {
   return _var->goal();
  }
  
  
  void setGoalVel(Position gv)
  {
   _var->setGoalVel(gv);
  }
  
  
  Position goalVel()
  {
   return _var->getGoalVel();
  }
};

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


#endif