ObsAvoiAlgoLIP.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 __OBS_AVOI_ALGO_LIP_H
#define __OBS_AVOI_ALGO_LIP_H

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

#include <algorithm>
#include "ObsAvoiAlgo.h"
#include <Spaces.h>
#include <Scan.h>
#include <DraWin.h>

using namespace MipResources;

namespace MipAlgorithms{
 
 
/* @{ */



class ObsAvoiAlgoLIPOptions : public Options {
 public:
  DecimalOption* driveGain; 
  DecimalOption* turnrateGain; 
  DecimalOption* influenceRadius; 
  DecimalOption* driveSatDw;
  DecimalOption* turnrateSatDw;
  DecimalOption* driveSatUp;
  DecimalOption* turnSatUp;
  DecimalOption* obstacleGrowing;
  DecimalOption* potentialTypeChangeDistance;
  DecimalOption* turnOffDistance;
  DecimalOption* angClusterAmp ;
  DecimalOption* minimaObstacleDistance;
  DecimalOption* turnrateVortexField;
  BoolOption* vortexField;
  ObsAvoiAlgoLIPOptions();
  
  string getObjectName() const {
   return "ObsAvoiAlgoLIPOptions";
  }

};


class ObsAvoiAlgoLIPParams : public MIPObject{
 ObsAvoiAlgoLIPOptions _options;
 
 public:
  DraWin* draWin; 
  Decimal driveGain; 
  Decimal turnrateGain; 
  Decimal influenceRadius; 
  Decimal driveSatDw;
  Decimal turnrateSatDw;
  Decimal driveSatUp;
  Decimal turnSatUp;
  Decimal obstacleGrowing;
  Decimal potentialTypeChangeDistance;
  Decimal turnOffDistance;
  Angle angClusterAmp;
  Decimal minimaObstacleDistance;
  bool vortexField;
  Decimal turnrateVortexField;
  //string mode;///when is equals to "trajecoryMode"obsAvoiAlgoLIP algorithm is used for a trajector following task else when it's equals to "goalBaseMode" the algorithm is used in a point to point task
  //Decimal pipeSection;///it's the section of tube that envolve the path.It's used only for trajectory tracking.Must be in meters
  
  ObsAvoiAlgoLIPParams(DraWin* dW=0,Decimal dG=1.0,Decimal tG=1.0,Decimal iR=1.0,Decimal dSDw=0.1,Decimal trSDw=0.05,Decimal dSU=10.0,Decimal trSU=10.0,Decimal oG=0.1,Decimal pTCD=0.5,Decimal GoalDistanceOFF=0.01, Angle angClusterAmp =0.0,Decimal mOd=0.01,bool vortexField=false,Decimal tGV=2.0);
  
  ObsAvoiAlgoLIPParams(DraWin* dW, int argc, const char* argv[]);
  
  string getObjectName() const {
   return "ObsAvoiAlgoLIP";
  }
};

class ObsAvoiAlgoLIPVars {
  
 public:
  unsigned int drawList; 
  vector<Position> localMinima; 
  Pose robotPose; 
  Position goal;
  Position totalForce;
  Position goalForce;
  Position repulsiveForce;
  Rays r;
  ObsAvoiAlgoLIPVars(){}
};


class ObsAvoiAlgoLIP : public ObsAvoiAlgo{
 private:
  ObsAvoiAlgoLIPVars   _var;
  ObsAvoiAlgoLIPParams _par; 
  
  stringstream _ssdebug;
  
//   vector <int> _clustering(Rays measures);//da vedere e mettere a punto
  Rays _Filtering(Scan &scan);//da vedere e mettere appunto
  void _draw();
  Position _goalForce(Position goal,Pose q);
  void _localMinimaFinder(Scan &scan,ObsAvoiAlgoIn in);
  Position _repulsiveForce(Decimal driveSat,Pose robotPose, vector<Position> &repulsivePoints,Pose sensPose); //compute artificial potential, the input is a minimum distance between robot and obstacle  
  void _cmdSaturation(ObsAvoiAlgoOut &out);//
  int minimaIndex(Ray data1,Ray data2,Ray data3,Ray data4);
 public:
  ObsAvoiAlgoLIP(ObsAvoiAlgoLIPParams inParams);
 
  void step(ObsAvoiAlgoIn in, ObsAvoiAlgoOut &out);
  
  void getControl(ObsAvoiAlgoIn &in, ObsAvoiAlgoOut *out){
   this->step(in, *out);
  }
  
  string getObjectName() const {
   return "ObsAvoiAlgoLIP";
  }
};

/* @} */


}; // end namespace MipAlgorithms


#endif