ObsAvoiAlgoSND.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_SND_H
#define __OBS_AVOI_ALGO_SND_H

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

#include <algorithm>
#include <ObsAvoiAlgo.h>
#include <Spaces.h>
#include <Scan.h>
#include <Option.h>
#include <LogTrace.h>
#include <list>

using namespace MipBaselib;

namespace MipAlgorithms{

/* @{ */

class ObsAvoiSNDGap {
 public:
  Angle bearing;
  Decimal distance;
  int iDir;
  
  ObsAvoiSNDGap(){};
  ObsAvoiSNDGap( Angle ang, Decimal d, int iD ){ bearing = ang; distance = d; iDir = iD; };
};

class ObsAvoiSNDValley {
 public:
  ObsAvoiSNDGap risingGap;
  ObsAvoiSNDGap otherGap;
  
  ObsAvoiSNDValley(){};
  ObsAvoiSNDValley( ObsAvoiSNDGap rG, ObsAvoiSNDGap oG ){ risingGap = rG; otherGap = oG; };
};


class ObsAvoiAlgoSNDOptions : public Options {
 public:

  DecimalOption* robotRadius;
  DecimalOption* obstacleAvoidDist;
  
  DecimalOption* maxSpeed;
  DecimalOption* maxTurnRate;
  DecimalOption* goalPositionTol;
  DecimalOption* goalAngleTol;
  DecimalOption* minGapWidth;
  
  BoolOption* trajectoryMode;
  
  ObsAvoiAlgoSNDOptions();
  
  string getObjectName() const {
   return "ObsAvoiAlgoSNDOptions";
  }

};


// class ObsAvoiAlgoSNDIn{
//  public:
//   Scan    scan; ///< Scan pointer.
//   Pose    goal;  ///< Goal position.
//   Pose    robotPose; ///< Robot pose.
//   Decimal rangeMax;///<Max linear range of sensor
//   
//   /// \brief  Constructor.
//   /// \param[in]&scanIn Scan pointer.
//   /// \param[in]&goalPoseIn Goal pose.
//   /// \param[in]&robotPoseIn Robot pose.
//   /// \param[in]&rangeMaxIn Max linear range of sensor
//   ObsAvoiAlgoSNDIn(Scan scanIn = Scan(), Pose goalPoseIn=Pose(0.0,0.0,0.0), Pose robotPoseIn=Pose(0.0,0.0,0.0),Decimal rangeMaxIn = 5.2);
// };



class ObsAvoiAlgoSNDParams : public MIPObject{
 ObsAvoiAlgoSNDOptions _options;
    
 public:
        
  Decimal robotRadius;
  Decimal obstacleAvoidDist;
  
  Decimal maxSpeed;
  Decimal maxTurnRate;
  Decimal goalPositionTol;
  Decimal goalAngleTol;
  Decimal minGapWidth;
  
  bool trajectoryMode; 
  
  ObsAvoiAlgoSNDParams(Decimal R=0.25,Decimal oAD=1.0,Decimal mS=0.5,Decimal mTR=1.5,Decimal gPT=0.25,Decimal gAT=6.3,Decimal mGW=0.6,bool mode=false);
  
  ObsAvoiAlgoSNDParams(int argc, const char* argv[]);
  
  string getObjectName() const {
   return "ObsAvoiAlgoSND";
  }
};

class ObsAvoiAlgoSNDVars {
 
 public:
  
  Pose robotPose; 
  Pose goal;
  
  std::vector<ObsAvoiSNDGap> gapVec;
  ObsAvoiSNDValley* pBestValley;
  Angle safeRisingGapAngle;
  Angle midValleyAngle;
  Decimal obsAvoidDelta;
  
  Angle driveAngle;
  ObsAvoiAlgoOut out;
  
  ObsAvoiAlgoSNDVars(){pBestValley = NULL;}
};


class ObsAvoiAlgoSND : public ObsAvoiAlgo{
 //private:
 public:
  ObsAvoiAlgoSNDVars   _var;
  ObsAvoiAlgoSNDParams _par;  
  
  stringstream _ssdebug;
  
  Rays _Filtering(Scan &scan);
  
  void _buildGapVector( Scan* pScan, Decimal fMaxRange );
  void _removeDuplicateGaps( Decimal fScanRes );
  void _findBestValley( Position distToGoal, Scan* pScan, Decimal fMaxRange );
  void _setObsAvoidDelta( Scan* pScan, Decimal safetyDist );
   
 public:
  ObsAvoiAlgoSND(ObsAvoiAlgoSNDParams inParams);
  
  void step(ObsAvoiAlgoIn in, ObsAvoiAlgoOut &out);
  
  void getControl(ObsAvoiAlgoIn &in, ObsAvoiAlgoOut *out){
   
   ObsAvoiAlgoIn inSND;
   
   inSND.goal = in.goal;
   inSND.rangeMax = 1.0;
   inSND.robPose = in.robPose;
   inSND.scan = in.scan;
   
   this->step(inSND, *out);
  }

  
  
  string getObjectName() const {
   return "ObsAvoiAlgoSND";
  }
};

/* @} */

}; // end namespace MipAlgorithms


#endif