Driver.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 __DRIVER_H_
#define __DRIVER_H_

#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <MobileRob3D.h>
#include <IPCommModule.h>
// #include <Parameter.h>
#include <CommonOptions.h>


#include <ObsAvoiAlgoPFM.h>
#include <ObsAvoiAlgoVFH.h>

#include <Serialization.h>
#include <SharedMem.h>





using namespace MipAlgorithms;
using namespace MipResources;

namespace MipTasks{
 /* @{ */
 
 enum DriverState{
  DRIVER_FIRST_RUN,
  DRIVER_NORMAL_RUN,
  DRIVER_NUM_RUNS
 };
 
 
 enum ObsAvoiMethod{
  NO=0, //no obstacle avoidance (only attractive control)
  PFM=1, //potential field method
  VFH=2, //vector field histogram
  OBSAVOI_NUM_METHODS
 };
 
 enum DriveInput{
  POSITION=0, // position input
  SPEED=1, // velocity input
  DRIVE_INPUT_NUM
 };
 
 
 class DriverOptions : public Options {
  public:
   IntOption* driverInput;
   IntOption* driverObsAvoi;
   DecimalOption* driverGoalX;
   DecimalOption* driverGoalY;
   DecimalOption* driverGoalZ;
   DecimalOption* driverGoalTheta;
   DecimalOption* driverMaxV;
   DecimalOption* driverMaxW;
   
   string getObjectName() const;
   
   DriverOptions();
 };
 
 
 
 class DriverSharedMem : public SharedMem{
  public:
   void initSharedMem();
   
   bool emergencyLock();
   
   void setControlInput(Decimal lv, Decimal av, Time t);
 };
 
 
 class DriverTaskPar {
  private:
  public:
   Decimal Vmax;
   Decimal Wmax;  
   Decimal goalReachedEps;
   Decimal Kv;
   Decimal Kw;
   Decimal Kwe;
   DriveInput driverInput;
   
   DriverTaskPar();
   
   DriverTaskPar(DriverOptions& options);
   
   DriverTaskPar(Decimal VmaxP, Decimal WmaxP, Decimal goalReachedEpsP, Decimal KvP, Decimal KwP, Decimal KweP,DriveInput driverInputP);
   
   DriverTaskPar(const DriverTaskPar &A);
   
   DriverTaskPar operator= (DriverTaskPar A);
 };
 
 
 class DriverTask : public Task{
  private:
   static const TaskPlate _plate = DRV_TASK;
   static const long int _maxDurSec  = 0, _maxDurUsec = 100000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 150000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 100000;
   
   static const long int TRACE_ON_DEFAULT = 0;
   static const long int COMM_TRACE_ON_DEFAULT = 0;
   static const long int BACK_VEL_DEFAULT = 0;
   
   MobileRob*  _mobileRob;
   MobileRob3D* _mobileRob3D;
   CommModule*  _commModule;
   
   bool useMobileRob;
   bool useMobileRob3D;
   
   DriverSharedMem* _sharedMem;
   
   bool commTraceOn;
   bool traceOn;
   
   DriverState state;
   DriverTaskPar par;
   ObsAvoiMethod method;
   
   bool backVelOn;
   
   ObsAvoiAlgo* OA;
   ObsAvoiAlgoIn OAin;
   ObsAvoiAlgoOut OAout;
   
   Pose robPose;
   Pose3D robPose3D;
   Scan scan;
   
   Pose goal;
   Pose3D goal3D;
   Position goalVel;
   
   Decimal traveledDistance;
   
   Decimal V;
   Decimal W;
   
   Timer controlPeriod;
   
   DriverOptions options;
   
   void init(ResourcePointers resources);
   void firstRun();
   void normalRun();
   
   void getCommPackets();
   void getPoseAndScan();
   void getPoseAndScan3D();
   void getGoal();
   void getGoal3D();
   void getGoalVel();
   void getControl();
   void drivePos();
   void driveVel();
   
   void attractiveControl();
   void velSat();
   
  public:
   DriverTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~DriverTask();
   
   TaskPlate getPlate() const {
    return _plate;
   }  
   Time getMaxDuration(){
    return Time(_maxDurSec,_maxDurUsec);
   }
   Time getMaxSamplPeriod(){
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   Time getMinSamplPeriod(){
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }
   
   TaskOutputs run();
 };
 
 /* @} */
 
};// end namespace MipTasks

#endif