ObsAvoiAlgoPFMKinect.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_PFM_H
#define __OBS_AVOI_ALGO_PFM_H

#include <ObsAvoiAlgo.h>

namespace MipAlgorithms{
 
/* @{ */



class ObsAvoiAlgoParPFMKinect : public ObsAvoiAlgoPar {
 
 public:
  
  Decimal ro;
  Decimal range;
  Decimal ka; 
  Decimal kb; 
  Decimal kr;
  Decimal gamma;
  
  ObsAvoiAlgoParPFMKinect(){
   ro = 0.4;
   range = 1;
   ka = 0.3;
   kb = ro*ka;
   kr = 0.002;
   gamma = 2;
  }
  
  ObsAvoiAlgoParPFMKinect(Decimal _ro, Decimal _range, Decimal _ka, Decimal _kr, Decimal _gamma){
   ro = _ro;
   range = _range;
   ka = _ka;
   kb = ro*ka;
   kr = _kr;
   gamma = _gamma;
  }

  ObsAvoiAlgoParPFMKinect(const ObsAvoiAlgoParPFMKinect &A){
   ro = A.ro;
   range = A.range;
   ka = A.ka;
   kb = A.kb;
   kr = A.kr;
   gamma = A.gamma;
  }
  
  ObsAvoiAlgoParPFMKinect operator= (ObsAvoiAlgoParPFMKinect A){
   ro = A.ro;
   range = A.range;
   ka = A.ka;
   kb = A.kb;
   kr = A.kr;
   gamma = A.gamma;
  }
 
};


class ObsAvoiAlgoPFMKinect {
  
 protected:
  
  ObsAvoiAlgoParPFMKinect par;
 
 private:
  Decimal Vr; 
  Decimal Wr; 
  Decimal Va; 
  Decimal Wa;
  float Fa_x;
  float Fa_y;
  float Fr_x;
  float Fr_y;

  void repulsiveControl(Pose nearestObstacle, Pose robot);
  
  void attractiveControl(Pose goal, Pose robot);

  void extractFromPose(Decimal&, Decimal&, Decimal&, const Pose*);
 
 public:
  
  ObsAvoiAlgoPFMKinect(){
   par = ObsAvoiAlgoParPFMKinect();
  }
  
  ObsAvoiAlgoPFMKinect(ObsAvoiAlgoParPFMKinect OAP){
   par = OAP;
  }

  void getForces(float& F_x, float& F_y);

  void getControl(Pose* obstacle, Pose goal, Pose robot, ObsAvoiAlgoOut *out);
  
  string getObjectName() const {
   return "ObsAvoiAlgoPFMKinect";
  }
};


/* @} */

}; // end namespace MipAlgorithms

#endif