PathPlanner.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 __PATH_PLANNER_
#define __PATH_PLANNER_

/* @{ */

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


#include <vector>
#include <Spaces.h>
#include <assert.h>
#include <math.h>
#include <Scan.h>
#include <TrajControl.h>
#include <ExplorationGraph.h>

#include <algorithm>

#include <Types.h>


using namespace std;
using namespace MipBaselib;

enum PathPlanners{
 RRT_PLANNER,
 PATH_PLANNERS_NUM
};

static const char* PathPlannersNames[PATH_PLANNERS_NUM] = {
 "RRT Planner"
};

enum PrimitivesType{
 STRAIGHT,
 TURN_LEFT,
 TURN_RIGHT,
 NULL_PRIMITIVE,
 PRIMITIVES_NUM
};

static const char* PrimitivesNames[PRIMITIVES_NUM] = {
 "Straight",
 "Turn Left",
 "Turn Right",
 "Null Primitive"
};

enum RRTstate{
 FIND_Q_RAND,
 FIND_Q_NEAR,
 FIND_Q_NEW,
 COLLISION_CHECK,
 UPDATE,
 GOAL_REACHED,
 FIND_PATH,
 SEARCH_COMPLETE,
 STATE_NUM
};

static const char* RRTstateNames[STATE_NUM] = {
 "Find qRand",
 "Find qNear",
 "Find qNew",
 "Collision check",
 "Update",
 "Goal reached",
 "Find Path",
 "Search Complete"
};

class RRTnode{
 private:
 public:
  int _ID;
  Pose _q;
  vector<int> _freePrimitives;
  int _freePrimitivesNum;
  int _parentID;
  int _edgeType;
  Decimal _CSpaceDist;
  Cell* _cell;
  Segment _straight;
  Ellipse _left;
  Ellipse _right;
//  public:
  RRTnode(){
   _parentID=-1;
   _freePrimitives=vector<int>(3,1);
   _freePrimitivesNum=3;
  }
  
  RRTnode(int ID,Pose q,int maxIter){
   _ID=ID;
   _q=q;
   _parentID=-1;
   _freePrimitives=vector<int>(3,1);
   _freePrimitivesNum=3;
  }
  
  
  RRTnode(int maxIter){
   _q=Pose(INFINITY,INFINITY,0.0);
   _parentID=-1;
   _freePrimitives=vector<int>(3,1);
   _freePrimitivesNum=3;
  }
  
  RRTnode(const RRTnode& n){
   _ID=n._ID;
   _q=n._q;
   _freePrimitives=n._freePrimitives;
   _freePrimitivesNum=n._freePrimitivesNum;
   _parentID=n._parentID;
   _straight=n._straight;
   _left=n._left;
   _right=n._right;
   _CSpaceDist=n._CSpaceDist;
   _cell=n._cell;
   _edgeType=n._edgeType;
  }
  RRTnode& operator=(const RRTnode& n){
   if (this != &n){
    _ID=n._ID;
    _q=n._q;
    _freePrimitives=n._freePrimitives;
    _freePrimitivesNum=n._freePrimitivesNum;
    _parentID=n._parentID;
    _straight=n._straight;
    _left=n._left;
    _right=n._right;
    _CSpaceDist=n._CSpaceDist;
    _cell=n._cell;
    _edgeType=n._edgeType;
   }
   return *this;
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  int ID(){
   return _ID;
  };
  
  Pose q(){
   return _q;
  };
  
  vector<int> freePrimitives(){
   return _freePrimitives;
  };
  
  int freePrimitivesNum(){
   return _freePrimitivesNum;
  };
  
  int parentID(){
   return _parentID;
  };
  
  int edgeType(){
   return _edgeType;
  };
  
  Decimal CSpaceDist(){
   return _CSpaceDist;
  };
  
  Cell* cell(){
   return _cell;
  };
  
  
  
  Segment straight(){
   return _straight;
  };
  
  Ellipse left(){
   return _left;
  };
  
  Ellipse right(){
   return _right;
  };
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<"ID\t\t"<<_ID<<endl;
   s<<"q\t\t"<<_q.print()<<endl;
   s<<"parent ID\t\t"<<_parentID<<endl;
   s<<"CSpaceDist\t\t"<<_CSpaceDist<<endl;
   return s.str();
  }
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setID(int value){
   _ID=value;
  };
  
  void setQ(Pose value){
   _q=value;
  };
  
  void setFreePrimitives(vector<int> value){
   _freePrimitives=value;
  };
  
  void clearFreePrimitivesElem(int value){
   _freePrimitives[value]=0;
  };
  
  void setFreePrimitivesNum(int value){
   _freePrimitivesNum=value;
  };
  
  void decreaseFreePrimitivesNum(){
   _freePrimitivesNum--;
  };
  
  void setParentID(int value){
   _parentID=value;
  };
  
  void setEdgeType(int value){
   _edgeType=value;
  };
  
  void setCSpaceDist(Decimal value){
   _CSpaceDist=value;
  };
  
  void setCell(Cell* value){
   _cell=value;
  };
  
  void setStraight(Segment value){
   _straight=value;
  };
  
  void setLeft(Ellipse value){
   _left=value;
  };
  
  void setRight(Ellipse value){
   _right=value;
  };
};

class RRTparams{
 private:
  int _maxIterations;
  Decimal _deltaT;
  Decimal _threshold;
  Decimal _probBiasedExpansion;
  vector<VelVec> _motionPrimitives;
 public:
  RRTparams(){}
  
  RRTparams(int maxIterations,Decimal delta,Decimal threshold,Decimal probBiasedExpansion/*,vector<VelVec> motionPrimitives*/){
   setMaxIterations(maxIterations);
   setDeltaT(delta);
   setThreshold(threshold);
   setProbBiasedExpansion(probBiasedExpansion);
   VelVec straight=VelVec(0.1,0.0,Time(0,0));
   _motionPrimitives.push_back(straight);
   VelVec turnLeft=VelVec(0.1,0.2,Time(0,0));
   _motionPrimitives.push_back(turnLeft);
   VelVec turnRight=VelVec(0.1,-0.2,Time(0,0));
   _motionPrimitives.push_back(turnRight);
//    setMotionPrimitives(motionPrimitives);
  }
  
  RRTparams(const RRTparams& params){
   _maxIterations=params._maxIterations;
   _deltaT=params._deltaT;
   _threshold=params._threshold;
   _probBiasedExpansion=params._probBiasedExpansion;
   _motionPrimitives=params._motionPrimitives;
  }
  RRTparams& operator=(const RRTparams& params){
   if (this != &params){
    _maxIterations=params._maxIterations;
    _deltaT=params._deltaT;
    _threshold=params._threshold;
    _probBiasedExpansion=params._probBiasedExpansion;
    _motionPrimitives=params._motionPrimitives;
   }
   return *this;
  }
  
  
  
  
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  int maxIterations(){
   return _maxIterations;
  };
  
  Decimal deltaT(){
   return _deltaT;
  };
  
  Decimal threshold(){
   return _threshold;
  };
  
  Decimal probBiasedExpansion (){
   return _probBiasedExpansion;
  };
  
  vector<VelVec> motionPrimitives(){
   return _motionPrimitives;
  };
  
/***************************************************************************************************
  ***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setMaxIterations(int value){
   _maxIterations=value;
  };
  
  void setDeltaT(Decimal value){
   _deltaT=value;
  };
  
  void setThreshold(Decimal value){
   _threshold=value;
  };
  
  void setProbBiasedExpansion (Decimal value){
   _probBiasedExpansion=value;
  };
  
  void setMotionPrimitives(vector<VelVec> value){
   _motionPrimitives=value;
  };
};


class RRT{
 private:
 public:
  RRTparams _params;
  Pose _qStart;
  Pose _qGoal;
  Pose _qRand;
  Pose _qNear;
//   Pose _qNew;
  RRTnode _tempNode;
//   int _parentID;
//   vector<Path*> _path;
  ExplorationNode* _expNode;
  vector<RRTnode> _tree;
  vector<int> _sortedByDistance;
  Decimal radius;
  Decimal lenght;
  Timer timer;
  Cell* _goalCell;
  
  int _nearest;
  Pose _nearestPose;
  int selPrim;
  
  
  vector<int> _pathNodes;//TODO Da levare
//  public:
  RRT(){}
  
  
  
  RRT(RRTparams params){
   setParams(params);
  }
  
  RRT(RRTparams params,Pose qStart,Pose qGoal,ExplorationNode* expNode){
   setParams(params);
   setQStart(qStart);
   setQGoal(qGoal);
   setExpNode(expNode);
//    _tempNode=RRTnode
   _tree.reserve(_params.maxIterations());

//    _tree=vector<RRTnode>(_params.maxIterations());
   initializeTree();
//    radius=fabs(_params.motionPrimitives()[TURN_LEFT].linVel()/_params.motionPrimitives()[TURN_LEFT].angVel());
//    lenght=_params.motionPrimitives()[STRAIGHT].linVel()*_params.deltaT();
   lenght=0.1;
   radius=0.1*sqrt(2.0);
   _goalCell=_expNode->grid().getCell(_qGoal.pos());
   _nearest=0;
  }
  
  
  
  RRT(const RRT& rrt){
   _params=rrt._params;
   _qStart=rrt._qStart;
   _qGoal=rrt._qGoal;
   _qRand=rrt._qRand;
   _qNear=rrt._qNear;
   _tempNode=rrt._tempNode;
   _expNode=rrt._expNode;
   _tree=rrt._tree;
   _sortedByDistance=rrt._sortedByDistance;
   radius=rrt.radius;
   lenght=rrt.lenght;
   timer=rrt.timer;
   _goalCell=rrt._goalCell;
   _nearest=rrt._nearest;
   selPrim=rrt.selPrim;
   _pathNodes=rrt._pathNodes;//TODO Da levare
  }
  RRT& operator=(const RRT& rrt){
   if (this != &rrt){
    _params=rrt._params;
    _qStart=rrt._qStart;
    _qGoal=rrt._qGoal;
    _qRand=rrt._qRand;
    _qNear=rrt._qNear;
    _tempNode=rrt._tempNode;
    _expNode=rrt._expNode;
    _tree=rrt._tree;
    _sortedByDistance=rrt._sortedByDistance;
    radius=rrt.radius;
    lenght=rrt.lenght;
    timer=rrt.timer;
    _goalCell=rrt._goalCell;
    _nearest=rrt._nearest;
    selPrim=rrt.selPrim;
    _pathNodes=rrt._pathNodes;//TODO Da levare
   }
   return *this;
  }
  
  
  
  
  
  
  
  
  
  
  
  void initializeTree();
  
  void findQrand();
  
  Decimal CSpaceDist(Pose q1, Pose q2);
  
  void findQnear();
  

  
  PrimitivesType findQnew();
  
  bool isCollisionFree(RRTnode node,Grid* grid);
  
  void update();
  
  bool buildRRT(vector<Path*> &path, int &complIter);
  
  bool buildRRT2(vector<Path*> &path, vector<Path*> &tree, int &complIter);
  
  
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  RRTparams params(){
   return _params;
  };
  
  Pose qStart(){
   return _qStart;
  };
  
  Pose qGoal(){
   return _qGoal;
  };
  
  Pose qRand(){
   return _qRand;
  };
  
  ExplorationNode* expNode(){
   return _expNode;
  };
  
/***************************************************************************************************
  ***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setParams(RRTparams value){
   _params=value;
  };
  
  void setQStart(Pose value){
   _qStart=value;
  };
  
  void setQGoal(Pose value){
   _qGoal=value;
  };
  
  void setQRand(Pose value){
   _qRand=value;
  };
  
  void setExpNode (ExplorationNode* value){
   _expNode=value;
  };
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   unsigned int treeSize=_tree.size();
   for(unsigned int i=0;i<treeSize;i++){
    s<<_tree[i].print()<<endl;
   }
   return s.str();
  }

};


// bool compareByCSpaceDist(RRTnode first, RRTnode second){
//  return (first.CSpaceDist()<second.CSpaceDist());
// }

class RTR{
 public:
  RTR();
  
  bool buildRTR(Angle &rotation, Segment &segment, Pose start, Pose goal);
};



#endif