ExplorationGraph.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 _EXPLORATION_GRAPH_H_
#define _EXPLORATION_GRAPH_H_

#include <Grid.h>

using namespace std;




enum CellComparisonSurvey{
 OBST_VS_OBST,//ok
 OBST_VS_FRONT,//conflict
 OBST_VS_UKWN,//ok
 OBST_VS_LSR,//conflict
 FRONT_VS_OBST,//conflict
 FRONT_VS_FRONT,//ok
 FRONT_VS_UKWN,//ok
 FRONT_VS_LSR,//ok
 UKWN_VS_OBST,//ok
 UKWN_VS_FRONT,//ok
 UKWN_VS_UKWN,//ok
 UKWN_VS_LSR,//ok
 LSR_VS_OBST,//conflict
 LSR_VS_FRONT,//ok
 LSR_VS_UKWN,//ok
 LSR_VS_LSR,//ok
 CELL_COMPARISON_SURVEY_NUM
};

static const char* CellComparisonSurveyName[CELL_COMPARISON_SURVEY_NUM] ={
 "Obstacle vs  Obstacle",
 "Obstacle vs Frontier",
 "Obstacle vs Unknown",
 "Obstacle vs LSR",
 "Frontier vs  Obstacle",
 "Frontier vs Frontier",
 "Frontier vs Unknown",
 "Frontier vs LSR",
 "Unknown vs  Obstacle",
 "Unknown vs Frontier",
 "Unknown vs Unknown",
 "Unknown vs LSR",
 "LSR vs  Obstacle",
 "LSR vs Frontier",
 "LSR vs Unknown",
 "LSR vs LSR",
};


enum AddNodeState{
 FIND_NEXT_NODE,
 UPDATE_NODE,
 FINALIZE_NODE,
 VERIFY,
 ADD_NODE_STATE_NUM
};

static const char* AddNodeStateName[ADD_NODE_STATE_NUM]={
 "Find Next Node",
 "Update Node",
 "Finalize Node",
 "Verify"
};


namespace MipBaselib {
int evalCoeff(Cell* cell);




class ExplorationNode{
 private:
 public:
  int _ID;         
  Scan _scan;        
  Grid _grid;        
  vector<int> _adjacency;  
  vector<int> _realAdjacency;  
  vector<Decimal> _weight; 
  Time _nodeTime;
  
  Decimal _gScore;
  Decimal _hScore;
  Decimal _fScore;
  int cameFrom;
  
  
  
 
  ExplorationNode(){}
  ExplorationNode(Scan scan, int ID, ExplorationParams params, Time nodeTime, Pose gridCenter);
  
  
  
  ExplorationNode(const ExplorationNode& node){
   _ID=node._ID;
   _scan=node._scan;
   _grid=node._grid;
   _adjacency=node._adjacency;
   _realAdjacency=node._realAdjacency;
   _weight=node._weight;
   _nodeTime=node._nodeTime;
  }
  ExplorationNode& operator=(const ExplorationNode& node){
   if (this != &node){
    _ID=node._ID;
    _scan=node._scan;
    _grid=node._grid;
    _adjacency=node._adjacency;
    _realAdjacency=node._realAdjacency;
    _weight=node._weight;
    _nodeTime=node._nodeTime;
   }
   return *this;
  }
  
  
  
  
  
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setGridScanParams(ExplorationParams value){
   _grid.setParams(value);
  }
  
  void setNodePose(ExplorationParams value){
   _grid.setParams(value);
  }
  
  void setID(int value){
   _ID=value;
  }

  void setScan(Scan value){
   _scan=value;
  }
  
  void setGrid(Grid value){
   _grid=value;
  }
  
  void insertAdjacency(int index,int value){
   _adjacency[index]=value;
  }
  
  void insertRealAdjacency(int index,int value){
   _realAdjacency[index]=value;
  }
  
  void insertWeight(int index,Decimal value){
   _weight[index]=value;
  }
  
  void pushBackAdjacency(int value){
   _adjacency.push_back(value);
  }
  
  void pushBackRealAdjacency(int value){
   _realAdjacency.push_back(value);
  }
  
  void pushBackWeight(Decimal value){
   _weight.push_back(value);
  }
  
  void setNodeTime(Time value){
   _nodeTime=value;
  }

/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  int ID(){
   return _ID;
  }

  Scan scan(){
   return _scan;
  }
  
  Grid grid(){
   return _grid;
  }
  
  Grid* gridPtr(){
   return &_grid;
  }
  
  vector<int> adjacency(){
   return _adjacency;
  }
  
  vector<int> realAdjacency(){
   return _realAdjacency;
  }
  
  vector<Decimal> weight(){
   return _weight;
  }
  
  Time nodeTime(){
   return _nodeTime;
  }
  
  string printAdj(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   for(int i=0;i<_adjacency.size();i++){
    s<<_adjacency[i]<<"\t";
   }
   s<<endl;
   return s.str();
  }
  
  string printWei(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   for(int i=0;i<_weight.size();i++){
    s<<_weight[i]<<"\t";
   }
   s<<endl;
   return s.str();
  }
  
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<_grid.print()<<endl;
   return s.str();
  }
};



class ExplorationGraph{
 private:
 public:
  vector<ExplorationNode> _graph;  
  ExplorationParams _params;    
  Timer _graphTimer;        
  Decimal _adjacTh;         
  vector<int> _Amatrix;       
  vector<Decimal> _Wmatrix;     
  Pose _nextVP;
   Grid globalGrid;
  
  vector<int> bounds;
  vector<int> arrived;
  Time deadLine;
  
  
  
  pthread_mutex_t mutex;
  void graphAccess() { pthread_mutex_lock(&mutex);}

  void graphRelease(){ pthread_mutex_unlock(&mutex);}
  
  bool isAdjacent(ExplorationNode* first,ExplorationNode* second);

  ExplorationNode getNode(int ID);
  
  ExplorationNode* getNodePtr(int ID);
  

  Time getNodeLastUpd(int ID);
  
  
  void updateAdj(ExplorationNode &first,ExplorationNode &second);
  
  void updateGlobalGrid(int nodeID);
  
  void finalizeGlobalGrid();
  
  void updateGlobalGrid2(int nodeID);
  
  void updateLocalGrids();


  bool addNode2(int &lastCompared,int newNodeID);
  bool addNode(int &lastCompared,int newNodeID);
  void addNode(Scan scan, Pose nodePose);
  void setNextVP(Pose nextVP){
   _nextVP=nextVP;
  }
  
  vector<int> AStar(int IDstart,int IDdest);
  
//  public:
  ExplorationGraph(){}
  ExplorationGraph(ExplorationParams params);
  ExplorationGraph(ExplorationParams params,Decimal adjacTh);

  ~ExplorationGraph();

  ExplorationGraph(const ExplorationGraph &g);
  
  ExplorationGraph operator=(const ExplorationGraph &g){
   if (this != &g){
    _graph=g._graph;
    _params=g._params;
    _graphTimer=g._graphTimer;
    _adjacTh=g._adjacTh;
    _Amatrix=g._Amatrix;
    _Wmatrix=g._Wmatrix;
   }
   return *this;
  }
  
  Decimal nodeDist(ExplorationNode first,ExplorationNode second);
  
  void compA();
  
  void compW();
  
};

bool compareFscore(ExplorationNode* first, ExplorationNode* second);


}; // end of namespace


#endif


      /*************************************************************/
      /*               Functions for path search                   */
      /*************************************************************/ 

//       Solution Homing(NodeMatrix sd, Solution Back, int numRobot);


      /*************************************************************/
      /*        Functions for Computing the Forward Trees          */
      /*************************************************************/

// /*inline*/ double* FloydAPSP();


  
/*************************************************************/
/*        Functions for Computing the Forward Trees          */
/*************************************************************/


//  double* FloydAPSP(ListNode* ListOfNodesToExclude){}


 /*************************************************************/
 /*               Functions for path search                   */
 /*************************************************************/ 



//                NM AdiacentNodes(IdNode nod);

//    vector<int> AStar(int IDstart,int IDdest);
  
//                bool ConflictChecking(Solution Sol);
  
//                IntMatrix Constraints(Solution Sol);
    
//                GraphPath Astar_tempo(Solution Par, IdNode s, IdNode d);
  
//                Solution Replanning(int* priority, Solution ott, NodeMatrix sd);
  
//                void PriorityChooser(int* prty, IntMatrix vincoli, int numRobot);  
  
//                void PriorityChooser(IntMatrix vincoli,int*pr, int numRobot);   
               

//                double SolutionCost(Solution Sol);





/***************************************************/
/*                  Utility funcion                */
/***************************************************/
//                void PrintLGraph(int robot);//TODO

//                void PrintLNodeWeights(int robot);//TODO

//     void WriteGraph(string nomefile);//TODO

//     void ReadGraph(string nomefile);//TODO
    
//                void Write(string nomefile);

//                void Read(string nomefile);