ExplorationGraphProva.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",
};

int evalCoeff(Cell* cell);

class ExplorationNode{
 private:
  int _ID;         
  Scan _scan;        
  Grid _grid;        
  vector<int> _adjacency;  
  vector<Decimal> _weight; 
  Time _nodeTime;
  
  
 public:
  ExplorationNode(Scan scan, int ID, ExplorationParams params, Time nodeTime, Pose gridCenter);
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setID(int value){
   _ID=value;
  }

  void setScan(Scan value){
   _scan=value;
  }
  
  void setGrid(Grid value){
   _grid=value;
  }
  
  void setAdjacency(vector<int> value){
   _adjacency=value;
  }
  
  void setWeight(vector<Decimal> value){
   _weight=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<Decimal> weight(){
   return _weight;
  }
  
  Time nodeTime(){
   return _nodeTime;
  }
};



class ExplorationGraph{
 private:
 public:
  vector<ExplorationNode> _graph;  
  ExplorationParams _params;    
  Timer _graphTimer;        
  Decimal _adjacTh;         
  vector<int> _Amatrix;       
  vector<Decimal> _Wmatrix;     
  Pose _nextVP;
  
  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);
  

  Time getNodeLastUpd(int ID);
  
  void updateAdj(ExplorationNode first,ExplorationNode second);


  void addNode(Scan scan);
  
  void setNextVP(Pose nextVP){
   _nextVP=nextVP;
  }
  
//  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();
  
};

#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);

//                GraphPath AStar(IdNode start, IdNode dest);
  
//                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);
    
    
    vector<Cell*> bresenhamCircle(ExplorationNode node, Cell* start, Cell* stop, Cell* center){
     vector<Cell*> bres;
 
     int iStart=start->indexes().i();
     int jStart=start->indexes().j();
     int iStop=stop->indexes().i();
     int jStop=stop->indexes().j();
     int iCenter=center->indexes().i();
     int jCenter=center->indexes().j();

     int Di1 = iStart-iCenter;
     int Dj1 = jStart-jCenter;
     int Di2 = iStop-iCenter;
     int Dj2 = jStop-jCenter;
 
     bool mSign1=((Di1<=0)^((Dj1<=0)));
     bool mMod1=(abs(Di1)<=abs(Dj1));
     bool increasing1;
     if (mMod1){
      increasing1=(Dj1>=0);
     }
     else{
      increasing1=(Di1>=0);
     }
 
     int octant1=4*increasing1+2*mMod1+mSign1;
 
     bool mSign2=((Di2<=0)^((Dj2<=0)));
     bool mMod2=(abs(Di2)<=abs(Dj2));
     bool increasing2;
     if (mMod2){
      increasing2=(Dj2>=0);
     }
     else{
      increasing2=(Di2>=0);
     }
     int octant2=4*increasing2+2*mMod2+mSign2;
 
 

     int j = jStart;
     int i = iStart;

 
 
     int leadIndex;
     int followIndex;
     int leadVariation;
     int followVariation;
     int currLead;
     int currFollow;
     int Dlead;
     int Dfollow;
     int leadArrival;
 
     int radius = (int)sqrt(Di1*Di1+Dj1*Dj1);
     int d= 1-radius;
     int deltaE;
     int deltaSE;
//  int index=jStart-1+((iStart-1)*columns());
 
//  bres.push_back(octant);
     bres.push_back(start);

 
 
 
     deltaE = 3;
     deltaSE = -2*radius+5;
//  CirclePoints(x, y, value);
     while ( j != jStop ) {
      if ( d < 0 ) {
       d = d+deltaE;
       deltaE = deltaE+2;
       deltaSE = deltaSE+2;
       j++;
      } else {
       d = d+deltaSE;
       deltaE = deltaE+2;
       deltaSE = deltaSE+4;
       j++;
       i--;
      }
      int index=j-1+((i-1)*node.grid().columns());
      bres.push_back(node.grid().getCell(index));
//   CirclePoints(x, y, value);
     }
    }