00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00030
00034
00036
00037
00038 #ifndef _EXPLORATION_GRAPH_H_
00039 #define _EXPLORATION_GRAPH_H_
00040
00041 #include <Grid.h>
00042
00043 using namespace std;
00044
00045
00046
00047
00048 enum CellComparisonSurvey{
00049 OBST_VS_OBST,
00050 OBST_VS_FRONT,
00051 OBST_VS_UKWN,
00052 OBST_VS_LSR,
00053 FRONT_VS_OBST,
00054 FRONT_VS_FRONT,
00055 FRONT_VS_UKWN,
00056 FRONT_VS_LSR,
00057 UKWN_VS_OBST,
00058 UKWN_VS_FRONT,
00059 UKWN_VS_UKWN,
00060 UKWN_VS_LSR,
00061 LSR_VS_OBST,
00062 LSR_VS_FRONT,
00063 LSR_VS_UKWN,
00064 LSR_VS_LSR,
00065 CELL_COMPARISON_SURVEY_NUM
00066 };
00067
00068 static const char* CellComparisonSurveyName[CELL_COMPARISON_SURVEY_NUM] ={
00069 "Obstacle vs Obstacle",
00070 "Obstacle vs Frontier",
00071 "Obstacle vs Unknown",
00072 "Obstacle vs LSR",
00073 "Frontier vs Obstacle",
00074 "Frontier vs Frontier",
00075 "Frontier vs Unknown",
00076 "Frontier vs LSR",
00077 "Unknown vs Obstacle",
00078 "Unknown vs Frontier",
00079 "Unknown vs Unknown",
00080 "Unknown vs LSR",
00081 "LSR vs Obstacle",
00082 "LSR vs Frontier",
00083 "LSR vs Unknown",
00084 "LSR vs LSR",
00085 };
00086
00087
00088 enum AddNodeState{
00089 FIND_NEXT_NODE,
00090 UPDATE_NODE,
00091 FINALIZE_NODE,
00092 VERIFY,
00093 ADD_NODE_STATE_NUM
00094 };
00095
00096 static const char* AddNodeStateName[ADD_NODE_STATE_NUM]={
00097 "Find Next Node",
00098 "Update Node",
00099 "Finalize Node",
00100 "Verify"
00101 };
00102
00103
00104 namespace MipBaselib {
00106 int evalCoeff(Cell* cell);
00107
00108
00109
00110
00118 class ExplorationNode{
00119 private:
00120 public:
00121 int _ID;
00122 Scan _scan;
00123 Grid _grid;
00124 vector<int> _adjacency;
00125 vector<int> _realAdjacency;
00126 vector<Decimal> _weight;
00127 Time _nodeTime;
00128
00129 Decimal _gScore;
00130 Decimal _hScore;
00131 Decimal _fScore;
00132 int cameFrom;
00133
00134
00135
00136
00137 ExplorationNode(){}
00138 ExplorationNode(Scan scan, int ID, ExplorationParams params, Time nodeTime, Pose gridCenter);
00139
00140
00141
00143 ExplorationNode(const ExplorationNode& node){
00144 _ID=node._ID;
00145 _scan=node._scan;
00146 _grid=node._grid;
00147 _adjacency=node._adjacency;
00148 _realAdjacency=node._realAdjacency;
00149 _weight=node._weight;
00150 _nodeTime=node._nodeTime;
00151 }
00153 ExplorationNode& operator=(const ExplorationNode& node){
00154 if (this != &node){
00155 _ID=node._ID;
00156 _scan=node._scan;
00157 _grid=node._grid;
00158 _adjacency=node._adjacency;
00159 _realAdjacency=node._realAdjacency;
00160 _weight=node._weight;
00161 _nodeTime=node._nodeTime;
00162 }
00163 return *this;
00164 }
00165
00166
00167
00168
00169
00170
00171
00172
00173
00175 void setGridScanParams(ExplorationParams value){
00176 _grid.setParams(value);
00177 }
00178
00180 void setNodePose(ExplorationParams value){
00181 _grid.setParams(value);
00182 }
00183
00185 void setID(int value){
00186 _ID=value;
00187 }
00188
00190 void setScan(Scan value){
00191 _scan=value;
00192 }
00193
00195 void setGrid(Grid value){
00196 _grid=value;
00197 }
00198
00200 void insertAdjacency(int index,int value){
00201 _adjacency[index]=value;
00202 }
00203
00205 void insertRealAdjacency(int index,int value){
00206 _realAdjacency[index]=value;
00207 }
00208
00210 void insertWeight(int index,Decimal value){
00211 _weight[index]=value;
00212 }
00213
00215 void pushBackAdjacency(int value){
00216 _adjacency.push_back(value);
00217 }
00218
00220 void pushBackRealAdjacency(int value){
00221 _realAdjacency.push_back(value);
00222 }
00223
00225 void pushBackWeight(Decimal value){
00226 _weight.push_back(value);
00227 }
00228
00230 void setNodeTime(Time value){
00231 _nodeTime=value;
00232 }
00233
00234
00235
00236
00238 int ID(){
00239 return _ID;
00240 }
00241
00243 Scan scan(){
00244 return _scan;
00245 }
00246
00248 Grid grid(){
00249 return _grid;
00250 }
00251
00253 Grid* gridPtr(){
00254 return &_grid;
00255 }
00256
00258 vector<int> adjacency(){
00259 return _adjacency;
00260 }
00261
00263 vector<int> realAdjacency(){
00264 return _realAdjacency;
00265 }
00266
00268 vector<Decimal> weight(){
00269 return _weight;
00270 }
00271
00273 Time nodeTime(){
00274 return _nodeTime;
00275 }
00276
00277 string printAdj(){
00278 stringstream s;
00279 s.precision(3);
00280 s.setf(ios::fixed,ios::floatfield);
00281 for(int i=0;i<_adjacency.size();i++){
00282 s<<_adjacency[i]<<"\t";
00283 }
00284 s<<endl;
00285 return s.str();
00286 }
00287
00288 string printWei(){
00289 stringstream s;
00290 s.precision(3);
00291 s.setf(ios::fixed,ios::floatfield);
00292 for(int i=0;i<_weight.size();i++){
00293 s<<_weight[i]<<"\t";
00294 }
00295 s<<endl;
00296 return s.str();
00297 }
00298
00299
00300 string print(){
00301 stringstream s;
00302 s.precision(3);
00303 s.setf(ios::fixed,ios::floatfield);
00304 s<<_grid.print()<<endl;
00305 return s.str();
00306 }
00307 };
00308
00309
00310
00318 class ExplorationGraph{
00319 private:
00320 public:
00321 vector<ExplorationNode> _graph;
00322 ExplorationParams _params;
00323 Timer _graphTimer;
00324 Decimal _adjacTh;
00325 vector<int> _Amatrix;
00326 vector<Decimal> _Wmatrix;
00327 Pose _nextVP;
00328 Grid globalGrid;
00329
00330 vector<int> bounds;
00331 vector<int> arrived;
00332 Time deadLine;
00333
00334
00335
00336 pthread_mutex_t mutex;
00338 void graphAccess() { pthread_mutex_lock(&mutex);}
00339
00340 void graphRelease(){ pthread_mutex_unlock(&mutex);}
00341
00345 bool isAdjacent(ExplorationNode* first,ExplorationNode* second);
00346
00350 ExplorationNode getNode(int ID);
00351
00355 ExplorationNode* getNodePtr(int ID);
00356
00357
00361 Time getNodeLastUpd(int ID);
00362
00363
00367 void updateAdj(ExplorationNode &first,ExplorationNode &second);
00368
00372 void updateGlobalGrid(int nodeID);
00373
00377 void finalizeGlobalGrid();
00378
00382 void updateGlobalGrid2(int nodeID);
00383
00387 void updateLocalGrids();
00388
00389
00393 bool addNode2(int &lastCompared,int newNodeID);
00394 bool addNode(int &lastCompared,int newNodeID);
00395 void addNode(Scan scan, Pose nodePose);
00399 void setNextVP(Pose nextVP){
00400 _nextVP=nextVP;
00401 }
00402
00406 vector<int> AStar(int IDstart,int IDdest);
00407
00408
00410 ExplorationGraph(){}
00412 ExplorationGraph(ExplorationParams params);
00414 ExplorationGraph(ExplorationParams params,Decimal adjacTh);
00415
00417 ~ExplorationGraph();
00418
00420 ExplorationGraph(const ExplorationGraph &g);
00421
00423 ExplorationGraph operator=(const ExplorationGraph &g){
00424 if (this != &g){
00425 _graph=g._graph;
00426 _params=g._params;
00427 _graphTimer=g._graphTimer;
00428 _adjacTh=g._adjacTh;
00429 _Amatrix=g._Amatrix;
00430 _Wmatrix=g._Wmatrix;
00431 }
00432 return *this;
00433 }
00434
00439 Decimal nodeDist(ExplorationNode first,ExplorationNode second);
00440
00443 void compA();
00444
00447 void compW();
00448
00449 };
00450
00451 bool compareFscore(ExplorationNode* first, ExplorationNode* second);
00452
00453
00454 };
00455
00456
00457 #endif
00458
00459
00460
00461
00462
00463
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00488
00489
00490
00491
00492
00493
00494
00495
00496
00502
00503
00510
00511
00517
00518
00524
00525
00533
00534
00542
00543
00550
00551
00558
00559
00560
00566
00567
00568
00569
00570
00571
00572
00573
00574
00579
00580
00585
00586
00591
00592
00597
00598
00603
00604
00609
00610
00611
00612
00613
00614
00615
00616