MRXT: The Multi-Robot eXploration Tool: src/architecture/planner/HybridPlanner.cpp Source File

00001 
00002 #include "HybridPlanner.h"
00003 #include "gridMapInterface.h"
00004 #include <fstream>
00005 #include <math.h>
00006 #include <stdio.h>
00007 #include <list>
00008 #include <vector>
00009 #include <algorithm>
00010 #include <opencv2/opencv.hpp>
00011 #include "StringTokenizer.h"
00012 
00013 #define ST_EXPLORE      0
00014 #define ST_ACT_LOC      1
00015 #define ST_PLANNED      2
00016 #define ST_LOOP_CLOSE   3
00017 
00018 // Para registrar la clase en la factoria
00019 namespace planners{
00020         planner* CreateHybridPlanner(const ConfigFile& conf){
00021                 return new HybridPlanner(conf);
00022         };
00023         const int HYBRIDPLANNER = 0 ;
00024         const bool registered = plannerFactory::Instance().Register(HYBRIDPLANNER, plannerCreator(CreateHybridPlanner));
00025 }
00026 
00027 using namespace std;
00028 
00029 // constructor
00030 HybridPlanner::HybridPlanner(const ConfigFile& config):
00031         planner(),
00032         badlocalized(false),
00033         state(ST_EXPLORE),
00034         endPlanner(true),
00035         actionradius            (config.read<float>("TREERADIUS")),
00036         eszdilationradius       (config.read<int>("TREEDILATIONRADIUS")),
00037         min_frontier_length     (config.read<int>("MIN_FRONTIER_LENGTH")),
00038         min_gateway_length      (config.read<int>("MIN_GATEWAY_LENGTH")),
00039         utility_radius          (config.read<float>("UTILITY_RADIUS"))
00040 {}
00041 
00042 // destructor
00043 HybridPlanner::~HybridPlanner() {stop();}
00044 
00045 // thread setup
00046 int HybridPlanner::setup(){
00047 //      printf("arranco planificador\n");
00048         prio = 50;
00049         state = ST_EXPLORE;
00050         badlocalized = false;
00051         return 0;
00052 }
00053 
00054 // thread on stop
00055 void HybridPlanner::onStop(){
00056 //      printf("[HYBRID] Stopping...\n");
00057         if (!endPlanner){
00058                 endPlanner = true;
00059 //              printf("endplanner=true, locking...\n");
00060                 closing.lock();
00061 //              printf("unlocking...\n");
00062                 closing.unlock();
00063         }
00064         printf("[HYBRID_PLANNER] [robot %d] Stopped\n", rbase->getNumber());
00065 }
00066 
00067 // thread main code
00068 void HybridPlanner::execute(){
00069         printf("[HYBRID_PLANNER] \t\t\t\t\t HYBRID_PLANNER (%d) running\n", rbase->getNumber());
00070 
00071         closing.lock();
00072         endPlanner = false;     
00073         int n,r;
00074         point goal;
00075         badlocalized = false;
00076         int nextStep=number;
00077         
00078 //  init logs
00079         FILE* plannerlog=0;
00080 //      avifile=0;
00081 //      printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>save avi file= %d\n",saveAviFile);
00082         if (logstr){
00083                 char myfilestr[100];
00084                 sprintf(myfilestr,"%splanR%d.log",logstr,number);
00085                 plannerlog = fopen(myfilestr,"w");
00086 //              if (saveAviFile){
00087 //                      sprintf(myfilestr,"outfiles/%sr%dvideoplan.avi",logstr,number); 
00088 //                      avifile =  cvCreateVideoWriter( myfilestr, -1, 1, cvSize(mySlam->getGMWidth(),mySlam->getGMHeight()),1);
00089 ////                    printf("avifile created\n");
00090 //              }
00091         }
00092         
00093 
00094 //      char mystr[250];
00095 //      if (showPlanner){
00096 //              sprintf(mystr,"plan%d.jpg",number);
00097 //              cvNamedWindow("PLAN",0);
00098 //      }
00099 
00100         bool endExploration = false;
00101         //printf("empieza el bucle\n");
00102 
00103         int planfailed = 0;
00104 
00105         while(!endPlanner && !endExploration) {
00106                 
00107                 // esperamos al slam
00108                 nextStep = mySlam->getStep()+15;
00109 //              printf("[HYBRID PLANNER] [robot %d planner] waiting for step %d\n", number, nextStep);
00110                 mySlam->waitForStep(nextStep);
00111 //              printf("[HYBRID PLANNER] [robot %d planner] processing step %d\n", number, nextStep);
00112 
00113                 if (nextStep > 20000) endExploration= true;
00114 
00115                 // cogemos los mapas y poses
00116                 gridMapInterface* omap  = mySlam->getOMap();
00117                 binMap* ppmap                   = mySlam->getPreMap();
00118                 binMap* ipmap                   = mySlam->getImpMap();
00119                 point poscell                   = mySlam->getCell(number);
00120                 int numrobots                   = mySlam->getNumRobots();
00121                 point* robotcells               = new point[numrobots]();
00122                 for(r=0; r<numrobots; r++)
00123                         robotcells[r] = mySlam->getCell(r);
00124 
00125                 // leer mensajes
00126                 string msg;
00127                 int msglen;
00128                 while((msglen=getMessage(msg))){
00129                         //cout << "Message: >> " << msg << endl;
00130                         StringTokenizer st(&msg[0]);
00131                         string messagetype(st.nextToken()); 
00132                         if (messagetype == "END"){
00133                                 endPlanner = true;
00134                         }
00135                 }
00136 
00137                 // antes de crear el arbol necesitamos saber si hay que contar fronteras o celdas precisas
00138                 //printf("[HYBRID PLANNER] [robot %d] Dispersion: %f\n", number, mySlam->getDisp(number));
00139                 badlocalized = mySlam->getLoc(number);
00140 
00141                 // Creamos el arbol de exploracion
00142                 //printf("[HYBRID PLANNER] [robot %d] creando arbol...\n", number);     
00143                 treeNode* tree = createExplorationTree(numrobots, robotcells, poscell, *omap, *ppmap, *ipmap);  
00144                 //printf("[HYBRID PLANNER] [robot %d] Ok - Nº de ramas principales: %d\n",number,tree->getNChildren());        
00145                 
00146                 // Evaluamos el arbol
00147                 float val,max=-99999999999999.9f;
00148                 evalTree(*tree);
00149                 //printf("evaluado\n");
00150                 max = tree->getValue();
00151                 
00152                 // Añadimos los nodos de primer nivel a la lista
00153                 std::vector<treeNode*> nodeList;
00154                 for (n = 0; n < tree->getNChildren(); n++){
00155                         val = tree->getChildren(n).getValue();
00156                         if (tree->getChildren(n).isLeaf()) {
00157                                 tree->getChildren(n).setValue(val*1.0f);                // factor de correccion para nodos hoja
00158                         }
00159                         nodeList.push_back(&tree->getChildren(n));
00160                 }
00161                 
00162                 // Correccion de valores si hay mas robots en la zona
00163                 float d2;
00164                 for(r=0; r < numrobots; r++){
00165                         if (r != number){
00166                                 if(tree->isRobot(r)){
00167                                         for (n = 0; n < tree->getNChildren(); n++){
00168                                                 d2 = pow((float)(robotcells[r].x-tree->getChildren(n).getX()),2)+pow((float)(robotcells[r].y-tree->getChildren(n).getY()),2);
00169                                                 tree->getChildren(n).setValue(tree->getChildren(n).getValue()*d2);
00170                                         }
00171                                 }
00172                         }
00173                 }
00174                 
00175                 // decidimos reactivo o planificado 
00176                 if (tree->getNChildren() > 0){
00177                         //printf("ordenar\n");          
00178                         std::sort(nodeList.begin(), nodeList.end(), treeNode::orderByValue);
00179                         //printf("ok\n");
00180                         if (nodeList.front()->getNodeType()==GATEWAY_NODE){             // planificado si el nodo de mas valor no es un nodo hoja
00181                                 state = ST_PLANNED;
00182                                 goal.x = nodeList.front()->getX();
00183                                 goal.y = nodeList.front()->getY();
00184                         }
00185                         else if (nodeList.front()->getNodeType()==LEAF_TYPE_FRONTIER){
00186                                 state = ST_EXPLORE;
00187                         }
00188                         else if (nodeList.front()->getNodeType()==LEAF_TYPE_PRECISE_POSE){
00189                                 state = ST_ACT_LOC;
00190                         }
00191                         if (max==0){
00192                                 if (planfailed >=2){
00193                                         endExploration = true;  // si el valor es cero acabamos
00194                                 }
00195                                 else{
00196                                         planfailed++;
00197                                 }
00198                         }
00199                         else{
00200                                 planfailed=0;
00201                         }
00202                 }
00203 
00204                 // Activamos el estado correspondiente en la capa reactiva
00205                 if (state == ST_EXPLORE){
00206                         //printf("************************************** [robot %d] EXPLORE ESZ\n",number);
00207         
00208                         reac->enableAvoidObstacles();
00209                         reac->enableAvoidOtherRobots();
00210                         reac->enableGoToFrontier();
00211                         reac->enableGoToUnexploredZones();
00212 
00213                         reac->disableGoToGoal();
00214                         reac->disableGoToPrecisePoses();
00215                 }
00216                 
00217                 if (state == ST_ACT_LOC){
00218                         //printf("************************************** [robot %d] ACTIVE LOCALIZATION \n",number);
00219                         reac->enableAvoidObstacles();
00220                         reac->enableGoToPrecisePoses();
00221 
00222                         reac->disableAvoidOtherRobots();
00223                         reac->disableGoToFrontier();
00224                         reac->disableGoToUnexploredZones();
00225                         reac->disableGoToGoal();
00226                 }
00227 
00228                 if (state == ST_PLANNED){
00229                         //printf("************************************** [robot %d] PLANNED \n",number);
00230                         reac->setGoal(goal);
00231                         reac->enableAvoidObstacles();
00232                         reac->enableGoToGoal();
00233 
00234                         reac->disableAvoidOtherRobots();
00235                         reac->disableGoToFrontier();
00236                         reac->disableGoToUnexploredZones();
00237                         reac->disableGoToPrecisePoses();                        
00238                 }
00239 
00240                 delete tree;
00241                 delete omap;
00242                 delete ppmap;
00243                 delete[] robotcells;
00244                 
00245                 // write logs
00246                 if (logstr){
00247                         fprintf(plannerlog,"%d, %d, %d, %d, %d\n",nextStep, state, badlocalized, poscell.x, poscell.y);
00248                 }
00249 
00250         }
00251 
00252         char sms[100];
00253         sprintf(sms,"END %d", getDir()); 
00254         sendMessage(string(sms),-1);
00255 
00256 //      if (showPlanner) cvDestroyWindow("PLAN");
00257 
00258         
00259         //closing logs
00260         if (logstr)     fclose(plannerlog);
00261 
00262 //      printf("[robot %d] stoping robot\n", number);
00263         reac->disableAll();
00264         reac->stop();
00265 
00266         closing.unlock();
00267         explorationFinished.step();
00268         
00269 }
00270 
00271 treeNode* HybridPlanner::createExplorationTree(int numrobots, const point* robotcells, const point& pos, const gridMapInterface& omap, const binMap& precisemap, const binMap& imprecisemap){
00272 //      printf("here 1\n");
00273 
00274         // nodo raiz
00275         treeNode* root_node = new treeNode(GATEWAY_NODE, numrobots);
00276         root_node->setCell(pos);
00277         root_node->setCost(0);
00278 //      printf("here 2\n");
00279 
00280         // zona procesada en blanco
00281         binMap processed(mySlam->getGMWidth(), mySlam->getGMHeight(), mySlam->getResolution(), mySlam->getXOrigin(), mySlam->getYOrigin());
00282 //      printf("here 3\n");
00283 
00284         // lista de nodos por procesar
00285         std::vector<treeNode*> nodeStorage;
00286 
00287         // puntero al siguiente nodo, empezamos por el raiz
00288         treeNode* node = root_node;
00289 //      printf("here 4\n");
00290 
00291 //      if (showPlanner) im = omap.getMapAsImage();
00292 
00293         int numnode=0;
00294 
00295         int actionradius_cells = ((int)floor(actionradius/omap.getResolution()+0.5));
00296         int utilityradius_cells = ((int)floor(utility_radius/omap.getResolution()+0.5));
00297 //      printf("here 5\n");
00298         do{
00299 
00300                 numnode++;              
00301                 // buscamos la ESZ del nodo
00302                 binMap newsafezone;
00303 //              printf("voy a hacer el esz\n");
00304                 if(node->isLeaf()){     
00305                         omap.esz(node->getX(), node->getY(), newsafezone, 0,utilityradius_cells);
00306                 }
00307 //              printf ("[%d] esz ok\n",numnode);
00308 //              if (showPlanner){
00309 //                      if (node == root_node){
00310 //                              for (int ii=newsafezone.getRoi().x-10; ii<newsafezone.getRoi().x + newsafezone.getRoi().width+10; ii++){
00311 //                                      for (int jj=newsafezone.getRoi().y-10; jj<newsafezone.getRoi().y + newsafezone.getRoi().height+10; jj++){
00312 //                                              if (newsafezone.get(ii,jj))
00313 //                                                      cvCircle(im,cvPoint(ii,newsafezone.getHeight()-1-jj),0,CV_RGB(255,255,0),-1);
00314 //                                      }
00315 //                              }
00316 //                              //show ROI
00317 //                              //cvRectangle(im, 
00318 //                              //      cvPoint(newsafezone.getRoi().x, newsafezone.getHeight()-1- newsafezone.getRoi().y),
00319 //                              //      cvPoint(newsafezone.getRoi().x + newsafezone.getRoi().width, newsafezone.getHeight()-1- (newsafezone.getRoi().y + newsafezone.getRoi().height)),
00320 //                              //      CV_RGB(0,0,255),1);
00321 //                      }
00322 //              }
00323 //                      printf("[%d] Door - Cost: %d\n",numnode,node->getCost());
00324 
00325                 else{                                                                                   // nodo puerta
00326                         omap.esz(node->getX(), node->getY(), newsafezone, eszdilationradius, actionradius_cells);
00327                         
00328                         // quitamos lo ya procesado
00329                         // newsafezone.sub(processed);
00330                         
00331                         binMap filter(newsafezone);
00332                         filter.sub(processed);
00333                         filter.removeUnconnected(node->getX(), node->getY());
00334                         filter.set(node->getX(), node->getY(), true);
00335                         // buscamos nodos hijo
00336                         seekChildren(numrobots, robotcells, newsafezone, filter, *node, omap, precisemap, imprecisemap);
00337                         for(int i = 0; i< node->getNChildren(); i++)
00338                                 nodeStorage.push_back(&node->getChildren(i));   // los añadimos a la lista
00339                         
00340                         // añadimos la nueva zona a la ya procesado
00341                         processed.add(filter);
00342                         
00343 //                      if (showPlanner){
00344 //                              cvCircle(im,cvPoint(node->getX(), omap.getHeight()-1-node->getY()),1,CV_RGB(0,255,255),-1);
00345 //                              if (node != root_node)
00346 //                                      cvLine(im,cvPoint(node->getX(),omap.getHeight()-1-node->getY()),cvPoint(node->getParent().getX(),omap.getHeight()-1-node->getParent().getY()),CV_RGB(0,0,255));
00347 //                      }       
00348                 }
00349 //              if(node->isLeaf()){                                                                                                     // nodo hoja
00350 //                      if (showPlanner){
00351 //                              if (node != root_node)
00352 //                                      cvLine(im,cvPoint(node->getX(),omap.getHeight()-1-node->getY()),cvPoint(node->getParent().getX(),omap.getHeight()-1-node->getParent().getY()),CV_RGB(0,0,255));
00353 //                              cvCircle(im,cvPoint(node->getX(),omap.getHeight()-1-node->getY()),1,CV_RGB(0,255,0),-1);
00354 //                      }
00355 ////                    printf("[%d] Leaf - Cost: %d\n",numnode,node->getCost());
00356 //              }
00357 //              printf("[%d] eval\n",numnode);
00358                 // evaluamos la zona
00359                 evalZone(numrobots, robotcells, newsafezone, *node, omap, precisemap, imprecisemap);
00360 //              printf("[%d] eval finished\n",numnode);
00361         
00362                 //printf("Storage size  = %d\n", nodeStorage.size());
00363 
00364                 // ordenamos de mas a menos coste y tomamos de la lista el de menor coste que quede
00365                 if (nodeStorage.size()>0){
00366                         //printf("sorting...\n");
00367                         std::sort(nodeStorage.begin(), nodeStorage.end(), treeNode::orderByCost);
00368                         node = nodeStorage.back();
00369                         nodeStorage.pop_back();
00370                 }
00371                 else
00372                         node = 0;
00373 
00374 //              printf("[%d] next node\n",numnode);
00375 
00376         }while(node);
00377 
00378 //      if (showPlanner){
00379 //              cvShowImage("PLAN",im);
00380 //              cvWaitKey(5);
00381 
00382 ////            printf("voy a hacer el frame grabe %d, %d\n",saveAviFile,avifile);
00383 //              if(saveAviFile && avifile){
00384 //                      int res = cvWriteFrame(avifile, im);
00385 //                      printf("frame grabber, result %d\n",res); 
00386 //              }
00387 ////            printf("acabo\n");
00388 
00389 //              cvReleaseImage(&im);
00390 //      }
00391         return root_node;
00392 }
00393 
00394 
00395 void HybridPlanner::seekChildren(int numrobots, const point* robotcells, const binMap& safezone, const binMap& filter, treeNode& node, const gridMapInterface& omap, const binMap& precisemap, const binMap& imprecisemap){
00396         //printf("[robot %d] seeking\n", number);
00397 
00398         // buscamos puertas
00399         binMap gateways;
00400         omap.gateways(safezone, gateways);
00401         gateways.times(filter);
00402 
00403 //      if (showPlanner){
00404 //              for (int ii=gateways.getRoi().x-10; ii<gateways.getRoi().x + gateways.getRoi().width+10; ii++){
00405 //                      for (int jj=gateways.getRoi().y-10; jj<gateways.getRoi().y + gateways.getRoi().height+10; jj++){
00406 //                              if (gateways.get(ii,jj))
00407 //                                      cvCircle(im,cvPoint(ii,gateways.getHeight()-1-jj),0,CV_RGB(255,0,0),-1);
00408 //                      }
00409 //              }
00410 //      }
00411         cluster(numrobots, robotcells, gateways, node, GATEWAY_NODE); // añadir los nodos puerta
00412 
00413         // buscamos nodos hoja
00414         if (!badlocalized){             // buscamos fronteras
00415                 binMap frontiers;
00416                 int nfront = omap.frontiersIn(filter, frontiers);
00417                 if (nfront>0) cluster(numrobots, robotcells, frontiers, node, LEAF_TYPE_FRONTIER); // añadir los nodos frontera
00418         //      binMap impreciseCells(filter);
00419         //      impreciseCells.times(imprecisemap);
00420         //      cluster(numrobots, robotcells, impreciseCells, node, LEAF_TYPE_IMPRECISE_POSE); // añadir los nodos de celda precisa
00421         }
00422         else{                           // buscamos celdas precisas
00423                 binMap preciseCells(filter);
00424                 preciseCells.times(precisemap);
00425                 cluster(numrobots, robotcells, preciseCells, node, LEAF_TYPE_PRECISE_POSE); // añadir los nodos de celda precisa
00426         }
00427 }
00428 
00429 int HybridPlanner::cluster(int numrobots, const point* robotcells, const binMap& map, treeNode& parent, int nodetype){
00430         int i,j;
00431         //printf("[robot %d] cluster\n", number);
00432         //mapa en blanco de celdas analizadas
00433         binMap aux(map.getWidth(), map.getHeight(), map.getResolution(), map.getXOrigin(), map.getYOrigin());
00434 
00435         door cl;
00436         int count = 0;  // numero de clusters encontrado
00437         float d;
00438 
00439         for (i = map.getRoi().x ; i < map.getRoi().x + map.getRoi().width ; i++){
00440                 for (j = map.getRoi().y ; j < map.getRoi().y + map.getRoi().height ; j++){   // for each cell
00441                         
00442                         std::list<point> seq;
00443                         clustering(i,j,aux,map,cl,seq,true);
00444                         
00445                         if (cl.scale >= min_gateway_length || (cl.scale >= min_frontier_length && (nodetype > LEAF_TYPE_FRONTIER))){ // cluster de al menos 3 celdas
00446 
00447                                 std::list<point>::iterator it;
00448                                 int k;
00449                                 for( k = 0, it = seq.begin(); it != seq.end() && k <= cl.scale/2; it++ , k++);
00450                                 
00451                                 it--;
00452                                 k--;
00453 
00454                                 cl.x = it->x;
00455                                 cl.y = it->y;
00456 
00457                         //      cl.x = cl.x/cl.scale;
00458                         //      cl.y = cl.y/cl.scale;
00459 
00460                                 count++;
00461                                 
00462                         //      printf("[robot %d] new node (%d, %d - length: %d, k: %d)\n",number, (int)cl.x, (int)cl.y, (int)cl.scale, k);
00463                                 treeNode* newNode = new treeNode(nodetype, numrobots); // new node
00464 
00465                                 newNode->setCell(point((int)cl.x,(int)cl.y));   // position
00466                                 //newNode->setCell(point(i,j)); // position
00467 
00468                                 d = EPS+sqrt(pow(((float)parent.getX())-cl.x,2)+pow(((float)parent.getY())-cl.y,2));
00469                                 //d = EPS+sqrt(pow(((float)parent.getX())-i,2)+pow(((float)parent.getY())-j,2));
00470                                 newNode->setCost(parent.getCost() + (int)d);                                            // cost
00471                                 //printf("Cost: %f\n",parent->getCost() + (int)d);
00472 
00473                                 parent.addChildren(*newNode);
00474                                 
00475                         }
00476                         cl.x=0;
00477                         cl.y=0;
00478                         cl.scale=0;
00479                 }
00480         }
00481 
00482         return count;
00483 }
00484 
00485 bool HybridPlanner::clustering(const int &i, const int &j, binMap& aux, const binMap& map, door& cl, std::list<point> &seq, bool backfront){
00486 
00487         if(aux.get(i,j)==false && map.get(i,j) == true ){  // if ipoint or door
00488                 point p(i,j);
00489 
00490                 if (backfront)
00491                         seq.push_back(p);
00492                 else
00493                         seq.push_front(p);
00494 
00495                 cl.x += i;
00496                 cl.y += j;
00497                 cl.scale++;
00498                 aux.set(i,j,true);
00499                 int x, y;
00500                 for (x = i-1; x<=i+1; x++){
00501                         for (y = j-1; y<=j+1; y++){
00502                                 if (clustering(x,y,aux,map,cl,seq,backfront)){
00503                                         backfront = !backfront;
00504                                 }
00505                         }
00506                 }
00507                 return true; 
00508         }
00509         else{
00510                 aux.set(i,j,true);
00511                 return false; 
00512         }
00513 }
00514 
00515 // busca robots, celdas precisas y celdas sin explorar en la zona
00516 void HybridPlanner::evalZone(int numrobots, const point* robotcells, const binMap& map, treeNode& node, const gridMapInterface& omap, const binMap& precisemap, const binMap& imprecisemap){
00517         // buscar celdas de interes en la nueva zona de vision
00518         int nIPoints=0;
00519         int nbots=0;
00520         
00521         int i,j,r;
00522 
00523 //      printf("[robot %d] %d,%d,%d,%d\n", number, map.roi.x, map.roi.y, map.roi.width, map.roi.height);
00524 
00525         node.clearRobots();
00526 
00527         for (i = map.getRoi().x ; i < map.getRoi().x + map.getRoi().width ; i++){
00528                 for (j = map.getRoi().y ; j < map.getRoi().y + map.getRoi().height ; j++){   // for each cell
00529                         if (map.get(i,j)){
00530                                 if (node.isLeaf()){
00531                                         if (badlocalized){
00532                                                 if(precisemap.get(i,j)){
00533                                                         nIPoints++;
00534                                                 }
00535                                         }
00536                                         else{
00537                                                 if(omap.isunknown(i,j)){
00538 
00539                                                         nIPoints++;
00540                                                 }
00541                                         }
00542                                 }
00543                                 for (r = 0; r<numrobots; r++){
00544                                         if (r!=number && robotcells[r].x == i && robotcells[r].y ==j ){
00545                                                 nbots++;
00546                                                 node.setRobot(r);
00547                                         }
00548                                 }
00549 
00550                                 if (node.getNodeType() == LEAF_TYPE_PRECISE_POSE){
00551                                         if(precisemap.get(i,j)){
00552                                                 nIPoints++;
00553                                         }
00554                                 }
00555                                 else if (node.getNodeType() == LEAF_TYPE_FRONTIER){
00556                                         if(omap.isunknown(i,j)){
00557                                                 nIPoints++;
00558                                         }
00559                                 }
00560                                 else if (node.getNodeType() == GATEWAY_NODE) {
00561                                         for (r = 0; r<numrobots; r++){
00562                                                 if (r!=number && robotcells[r].x == i && robotcells[r].y ==j ){
00563                                                         nbots++;
00564                                                         node.setRobot(r);
00565                                                 }
00566                                         }
00567                                 }
00568                         }
00569                 }
00570         }
00571         node.setiPoints(nIPoints);
00572         node.setnBots(nbots);
00573 }
00574 
00575 void HybridPlanner::evalTree(treeNode& tree){
00576         if (tree.isLeaf()){
00577                 tree.setValue(((float)tree.getiPoints()) / pow((float)tree.getCost(),2));
00578         }
00579         else{
00580                 float val,max=0;
00581                 for (int n = 0; n < tree.getNChildren(); n++){
00582                         evalTree(tree.getChildren(n));
00583                         val = tree.getChildren(n).getValue();
00584                         if (val>=max) max = val;
00585                 }
00586                 tree.setValue(max/(tree.getnBots()+1.0f));
00587         }
00588 }
00589 
00590 
00591