ObstacleAvoidance.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 __OBSTACLE_AVOIDANCE_H_
#define __OBSTACLE_AVOIDANCE_H_

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


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

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


using namespace MipAlgorithms;
using namespace MipResources;


namespace MipTasks{
 
 /* @{ */

 
 enum ObstacleAvoidanceState{
  obstacleAvoidance_FIRST_RUN,
  obstacleAvoidance_OBS_AV_NORMAL_RUN,
  obstacleAvoidance_NUM_RUNS
 };
 
 
 enum ObstacleAvoidanceMethods{
  OBS_AV_NO, //no obstacle avoidance (only attractive control)
  OBS_AV_LIP, //Local Incremental Planning
  OBS_AV_PFM, //potential field method
  OBS_AV_VFH, //vector field histogram
  OBS_AV_SND, //vector field histogram
  OBS_AV_NUM_METHODS
 };
 
 
 enum ObstacleAvoidanceInputTypes{
  OBS_AV_POSITION, // position input
  OBS_AV_SPEED, // velocity input
  OBS_AV_NUM_INPUT_TYPES
 };
 
 
 class ObstacleAvoidanceOptions : public Options {
  public:
   StringOption*  input; 
   DecimalOption* obstacleAvoidanceGoalX; 
   DecimalOption* obstacleAvoidanceGoalY; 
   DecimalOption* obstacleAvoidanceGoalZ; 
   DecimalOption* obstacleAvoidanceGoalTheta; 
   StringOption*  obstacleAvoidanceMethod; 
 
   string getObjectName() const {
    return "ObstacleAvoidanceOptions";
   }

   ObstacleAvoidanceOptions(){
    input    = createStringOption("obstacleAvoidanceInput",  "Choose among position (default) and speed (drive,turnrate) input", "position");
    obstacleAvoidanceGoalX    = createDecimalOption("obstacleAvoidanceGoalX",  "X coordinate of the Goal pose",                0.0);
    obstacleAvoidanceGoalY    = createDecimalOption("obstacleAvoidanceGoalY",  "Y coordinate of the Goal pose",                0.0);
    obstacleAvoidanceGoalZ    = createDecimalOption("obstacleAvoidanceGoalZ",  "Z coordinate of the Goal pose",                0.0);
    obstacleAvoidanceGoalTheta  = createDecimalOption("obstacleAvoidanceGoalTheta","Angle (rad) of the Goal pose",                0.0);
    obstacleAvoidanceMethod    = createStringOption("obstacleAvoidanceMethod", "Choose among NO (NO obstacle avoidance, only actractive potential), PFM (Potential Field Method), LIP (default)(Local Incremental Planning), VFH (Vector Field Histogramm)", "LIP");
    updateValues();
   }
 };
 
 
 class ObstacleAvoidanceSharedMem : public SharedMem{
  public:
 };
 
 
 class ObstacleAvoidancePar {
  private:
  public:
   Decimal goalReachedEps;
   Decimal Kv;
   Decimal Kw;
   Decimal Kwe;
   ObstacleAvoidanceInputTypes obAvInputType;
   
   ObstacleAvoidancePar(){
    
    goalReachedEps=0.03;
    
    Kv=0.3;
    Kw=0.8;
    Kwe=0.3;
    
    obAvInputType=OBS_AV_POSITION;
   }
   
   ObstacleAvoidancePar(ObstacleAvoidanceOptions& options){
    
    if (options.obstacleAvoidanceMethod->getValue()=="VFH") goalReachedEps=0.07;
    else goalReachedEps=0.03;
    
    Kv=0.3;
    Kw=0.8;
    Kwe=0.3;
    
    obAvInputType = OBS_AV_NUM_INPUT_TYPES;
    if(options.input->getValue() == "speed"){
     obAvInputType=OBS_AV_SPEED;
    } else if (options.input->getValue() == "position"){
     obAvInputType=OBS_AV_POSITION;
    }
    assert(obAvInputType < OBS_AV_NUM_INPUT_TYPES);
   }
   
   ObstacleAvoidancePar(Decimal VmaxP, Decimal WmaxP, Decimal goalReachedEpsP, Decimal KvP, Decimal KwP, Decimal KweP,ObstacleAvoidanceInputTypes obstacleAvoidanceInputP){
    goalReachedEps=goalReachedEpsP;
    Kv=KvP;
    Kw=KwP;
    Kwe=KweP;
    obAvInputType=obstacleAvoidanceInputP;
   }
 
   ObstacleAvoidancePar(const ObstacleAvoidancePar &A){
    goalReachedEps=A.goalReachedEps;
    Kv=A.Kv;
    Kw=A.Kw;
    Kwe=A.Kwe;
    obAvInputType=A.obAvInputType;
   }
   
   ObstacleAvoidancePar operator= (ObstacleAvoidancePar A){
    goalReachedEps=A.goalReachedEps;
    Kv=A.Kv;
    Kw=A.Kw;
    Kwe=A.Kwe;
    obAvInputType=A.obAvInputType;
   }
 };
 
 
 class ObstacleAvoidance : public Task{
  private:
   static const TaskPlate _plate = OBA_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;
   
   ObstacleAvoidanceSharedMem* _sharedMem;
   
   bool commTraceOn;
   bool traceOn;
   
   
 #ifndef MIP_PLATFORM_ARM
   DraWin* _draWin;
   unsigned int _drawList; 
   bool         dwOn;
 #endif
 
   void draw();
   
   
   ObstacleAvoidanceState state;
   ObstacleAvoidancePar par;
   ObstacleAvoidanceMethods method;
   
   bool backVelOn;
   
   ObsAvoiAlgoSNDParams* sndParams;
   ObsAvoiAlgoLIPParams* lipParams;
   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;
   
   ObstacleAvoidanceOptions options;
   
   void firstRun();
   void normalRun();
   
   void getCommPackets();
   void getPoseAndScan();
   void getGoal();
   void getGoalVel();
   void getControl();
   void drivePos();
   void driveVel();
   
   void attractiveControl();
   void velSat();
   
  public:
   ObstacleAvoidance(ResourcePointers resources,int argc, const char* argv[]);
   
 //   ObstacleAvoidance(ResourcePointers resources, ObstacleAvoidancePar P){
 //    init(resources);
 //    par = P;
 //   }
   
   ~ObstacleAvoidance(){
    if (traceOn) cout << "destr ObstacleAvoidance" << endl;
   }
   
   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(){
    switch(state){
     case obstacleAvoidance_FIRST_RUN:
      firstRun();
      state = obstacleAvoidance_OBS_AV_NORMAL_RUN;
      break;
     case obstacleAvoidance_OBS_AV_NORMAL_RUN:
      normalRun();
      break;
    }
    return TASK_OUT_GO_ON;
   }
 };
 
 /* @} */
 
};// end namespace MipTasks

#endif