ObsAvoiAlgoVFH.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_VFH_H
#define __OBS_AVOI_ALGO_VFH_H

#include <ObsAvoiAlgo.h>
#include "AlgoVFH.h"

namespace MipAlgorithms{
 
/* @{ */



class ObsAvoiAlgoParVFH : public ObsAvoiAlgoPar {
 
 public:  
  Decimal cellSize;           
  int windowDiameter;        
  int sectorAngle;         
  Decimal safetyDist0ms;         
  Decimal safetyDist1ms;         
  int maxSpeed;         
  int maxSpeedNarrowOpening;        
  int maxSpeedWideOpening;       
  int maxAcceleration;       
  int minTurnrate;         
  int maxTurnrate0ms;         
  int maxTurnrate1ms;         
  Decimal minTurnRadiusSafetyFactor;      
  Decimal freeSpaceCutoff0ms;      
  Decimal obsCutoff0ms;         
  Decimal freeSpaceCutoff1ms;      
  Decimal obsCutoff1ms;         
  Decimal weightDesiredDir;         
  Decimal weightCurrentDir;         
  Decimal goalDistTolerance;      
  Decimal robotRadius;         
  
  
  ObsAvoiAlgoParVFH();
  
  ObsAvoiAlgoParVFH(Decimal cellSizeP,
           int windowDiameterP, 
           int sectorAngleP, 
           Decimal safetyDist0msP,
           Decimal safetyDist1msP,
           int maxSpeedP, 
           int maxSpeedNarrowOpeningP,
           int maxSpeedWideOpeningP, 
           int maxAccelerationP, 
           int minTurnrateP, 
           int maxTurnrate0msP, 
           int maxTurnrate1msP,
           Decimal minTurnRadiusSafetyFactorP, 
           Decimal freeSpaceCutoff0msP,
           Decimal obsCutoff0msP,
           Decimal freeSpaceCutoff1msP,
           Decimal obsCutoff1msP,
           Decimal weightDesiredDirP, 
           Decimal weightCurrentDirP, 
           Decimal goalDistToleranceP, 
           Decimal robotRadiusP);
  
  ObsAvoiAlgoParVFH(const ObsAvoiAlgoParVFH &A);
  
  ObsAvoiAlgoParVFH operator= (ObsAvoiAlgoParVFH A);
};


class ObsAvoiAlgoVFH : public ObsAvoiAlgo {
  
 protected:
  
  ObsAvoiAlgoParVFH par;
 
 private:
  
  VFH_Algorithm *vfhAlgorithm;
  void init();
  
  void getLaserRanges(Scan& scan, Decimal laserRanges[361][2], int laserCount);
  
  void subSampleScan(Scan& scan, Angle angStart, Angle angRes, int samples);
  
 public:
  
  ObsAvoiAlgoVFH(){
   par = ObsAvoiAlgoParVFH();
   init();
  }
  
  ObsAvoiAlgoVFH(ObsAvoiAlgoParVFH OAP){
   par = OAP;
   init();
  }

  void getControl(ObsAvoiAlgoIn &in, ObsAvoiAlgoOut *out);
  
  
  string getObjectName() const {
   return "ObsAvoiAlgoVFH";
  }
};

/* @} */


}; // end namespace MipAlgorithms


#endif