MobileRob.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_H_
#define __MOB_ROB_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 <GroundSens.h>
#include <MotionModule.h>
#include <PositionDetector.h>

#include <Resource.h>


namespace MipResources{
 /* @{ */
 
 
 static void _mobileRobMotionWork ( void* p );
 
 static void _mobileRobMotionClean ( void* p );


class MobileRobOptions : public Options {
 public:
  IntOption* motionModule;
  IntOption* laser;
  IntOption* infrared;
  IntOption* sonar;
  IntOption* gps;
  IntOption* odoLoc;
  PoseOption *initialGoal;
  
  MobileRobOptions();
  string getObjectName() const {
   return "MobileRobOptions";
  }
  OptionGroupsType getGroup();
};


class MobileRobPar{
 private:
  int      _id;     
  Decimal  _radius; 
  string  _name;   
 protected:
  void setId(int i){
   assert(i>0);
   _id=i;
  }
  void setRadius(Decimal r){
   assert(r>0.0);
   _radius=r;
  }
  void setName(string n){
   _name=n;
  }
 public:
  MobileRobPar (){
   _id   = 0;
   _radius = 0.0;
   _name  = string();
  }
  MobileRobPar(const MobileRobPar &r){
   _id   = r._id;
   _radius = r._radius;
   _name  = r._name;
  }
  MobileRobPar& operator=(const MobileRobPar& r){
   if (this != &r){
    _id   = r._id;
    _radius = r._radius;
    _name  = r._name;
   }
   return *this;
  }
  int id(){
   return _id;
  }
  Decimal radius(){
   return _radius;
  }
  string name(){
   return _name;
  }
  string print(){
   stringstream s;
   s << "name " << name() << endl
    << "id " << _id << endl
    << "radius " << _radius << endl;
   return s.str();
  }
};


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


class MobileRob : public Resource{
 protected:
  MobileRobOptions options; 
  MobileRobPar   *_par;  
  MobileRobVar   *_var; 
  // onboard sensors and actuators
  RangeSens     *_rangeSens;     
  Unicycle     *_unicycle;      
  RangeSens     *_infrared;      
  GroundSens     *_groundInfrared;   
  RangeSens     *_sonar;       
  PositionDetector *_gps;        
  ScanTypes rangeSensorType;  
  ScanTypes infraredType;   
  bool rangeSensActive;  
  bool infraredActive;  
  bool unicycleActive;  
  bool sonarActive;    
  bool gpsActive;     
  bool useOdometryLocalization; 
  bool goalIsSet;
  
  Thread _motionThread;
  bool _motionThreadActive;
  bool _motionThreadPause;
  
  ofstream *outputFile;
  
  
 public:
  MobileRob(){
  }
  
  MobileRob(int argc, const char* argv[]);
  
  virtual MobileRobPar* robPar(){
   return _par;
  }
  
  virtual MobileRobVar* robVar(){
   return _var;
  }
  
  void activateMotionThread();
  
  void pauseMotionThread();

  void unpauseMotionThread();

  bool isPauseMotionThread();
  
  /* ### ### range scan ### ### */
  
  virtual RangeSensPar* rangeSensPar(){
   if (rangeSensActive){
    return _rangeSens->par();
   } else {
    cout << "MIP-WARNING (MobileRob): request for range sensor parameters that is not available." << endl;
    return NULL;
   }
  }
  
    
  virtual RangeSensmmPar* rangeSensmmPar(){
   if (rangeSensActive) {
    return _rangeSens->parmm();
   } else {
    warning("Request for RangeSensmmPar while RangeSens is not active.");
    return NULL;
   }
  }
  
  virtual bool updatedScan(Scan& s);
  
  virtual bool updatedScan(Scan& s,MotionModuleTState& state);
  
  virtual bool updatedState(MotionModuleTState& state, Time& timeStamp);
  
  virtual bool updatedState(MotionModuleTState& state);
  
  virtual bool updatedLastState(MotionModuleTState& state, Time& timeStamp, const Decimal &delta);
  
  virtual bool updatedLastState(MotionModuleTState& state, const Decimal &delta);
  
  virtual void startScan(unsigned int num){
   if (rangeSensActive) {
    _rangeSens->startScan(num);
   } else {
    warning("Request for startScan while RangeSens is not active.");
   }
  }
  
  virtual void stopScan(){
   if (rangeSensActive) {
    _rangeSens->stopScan();
   } else {
    warning("Request for stopScan while RangeSens is not active.");
   }
  }
  
  virtual bool storedScan(Scan& s,Time timeout){
   if (rangeSensActive) {
    return _rangeSens->storedScan(s,timeout);
   } else {
    warning("Request for storedScan while RangeSens is not active.");
    return false;
   }
  }
 
  /* ### ### unycycle ### ### */
  
  virtual UnicyclePar* unicyclePar(){
   return (UnicyclePar*) _unicycle->getPar();
  }
  
  virtual void velCmd(Decimal drive, Decimal turnrate){
   
   if (commonOptions->saveFiles->getValue()) {
    Time nowTime;
    (*outputFile) << nowTime.sec() << " " << nowTime.usec() << " " << drive << " " << turnrate << endl;
   }
   
   _unicycle->setCommands(drive, turnrate);
  }
  
  virtual void getVel(Decimal& linVel, Decimal& angVel){
   MotionModuleTState state = _unicycle->getState();
   linVel = state.linVel;
   angVel = state.angVel;
  }
  
  virtual Pose getPose(){
   return _unicycle->getState().pose;
  }
  
  virtual  MotionModuleTState getMotionState(){
   return _unicycle->getState();
  }
  
  void getCurvature(Decimal &c,bool &infinity){
   return _unicycle->getCurvature(c,infinity);
  }
  

  
  virtual bool setMotionState(MotionModuleState state, bool setPastTimeStamp=false, Time pastTimeStamp=Time()){
   _unicycle->setState(state,setPastTimeStamp,pastTimeStamp);
  }
  
  
  virtual void setPose(Pose p){
   MotionModuleTState state(p,0.0,0.0);
   _unicycle->setState(state);
  }
  

  /* ### ### Range infrared ### ### */

  virtual bool updatedIRScan(Scan& scan){
   if (infraredActive){
    _infrared->updatedScan(scan);
    return true;
   }
   return false;
  }
  
  /* ### ### Ground infrared ### ### */

  virtual bool updatedIRGroundScan(vector<int>& s){
   if (infraredActive){
    _groundInfrared->updatedScan(s);
    return true;
   }
   return false;
  }
  
  /* ### ### sonar ### ### */
  virtual bool getSonarMeasures(Scan &scan){
   if (sonarActive){
    _sonar->updatedScan(scan);
    return true;
   }
   return false;
  }
  
  /* ### ### gps ### ### */
  
  virtual bool getGPSMeasures(Position pos){
   if (gpsActive){
    pos = _gps->getMeasure();
    return true;
   }
   return false;
  }
  
  /* ### ### MobileRob ### ### */
  
  virtual void setGoal(Pose g){
   _var->setGoal(g);
   goalIsSet = true;
  }
  
  Pose getGoal(){
   if(goalIsSet){
    return _var->goal();
   }else{
    _var->setGoal(options.initialGoal->getValue());
    goalIsSet = true;
   }
  }
  
  void setGoalVel(Position gv);
  
  Position goalVel();
  
  bool getRangeSensActive();

                void setRangeSensActive(bool set);
  
  bool getInfraredActive();
  
  bool getUnicycleActive();
  
  bool getSonarActive();
  
  bool getGpsActive();
  
  
  bool getRangeSensorType(ScanTypes& type){
   if (rangeSensActive) {
    type = rangeSensorType;
    return true;
   }else{
    warning("Request for getRangeSensorType while RangeSens is not active.");
    return false;
   }
  }
  
  /*Delete these unuseful methods*/
  
  virtual int totScans(int lastTakenScan){
   return lastTakenScan+1;
  }
  

  
  virtual void sendString(string s){}
  
  virtual void turnLaserOn(){}
  
  /*End Delete these unuseful methods*/
  
};

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


#endif