|
MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
|
MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
00001 00002 #include "MarketPlanner.h" 00003 #include "gridMapInterface.h" 00004 #include <fstream> 00005 #include <math.h> 00006 #include <stdio.h> 00007 #include <list> 00008 #include <vector> 00009 #include <opencv2/opencv.hpp> 00010 #include "StringTokenizer.h" 00011 00012 // Para registrar la clase en la factoria 00013 namespace planners{ 00014 planner* CreateMarketPlanner(const ConfigFile& conf){ 00015 return new MarketPlanner(conf); 00016 }; 00017 const int MARKETPLANNER = 5 ; 00018 const bool registered = plannerFactory::Instance().Register(MARKETPLANNER, plannerCreator(CreateMarketPlanner)); 00019 } 00020 00021 using namespace std; 00022 00023 // Constructor 00024 MarketPlanner::MarketPlanner(const ConfigFile& config): 00025 planner(), 00026 endPlanner(true), 00027 replanning_period(config.read<float>("REPLANNING_PERIOD")), 00028 inflate_obstacles(config.read<int>("INFLATE_OBSTACLES")), 00029 utility_radius(config.read<float>("UTILITY_RADIUS")), 00030 utility_weight(config.read<float>("UTILITY_WEIGHT")), 00031 cost_weight(config.read<float>("COST_WEIGHT")) 00032 {} 00033 00034 // Destructor 00035 MarketPlanner::~MarketPlanner(){stop();} 00036 00037 // thread Setup 00038 int MarketPlanner::setup(){ 00039 00040 // printf("arranco planificador\n"); 00041 completedPath = true; 00042 prio = 50; 00043 return 0; 00044 } 00045 00046 // thread on stop 00047 void MarketPlanner::onStop(){ 00048 if (!endPlanner){ 00049 endPlanner = true; 00050 closing.lock(); 00051 closing.unlock(); 00052 } 00053 printf("[MARKETPLANNER] [robot %d] Stopped\n", rbase->getNumber()); 00054 } 00055 00056 // thread main execution 00057 void MarketPlanner::execute(){ 00058 printf("[MARKETPLANNER] \t\t\t\t\t MARKETPLANNER (%d) running\n", rbase->getNumber()); 00059 closing.lock(); 00060 endPlanner = false; 00061 point goal; 00062 int nextStep=number; 00063 00064 FILE* plannerlog=0; 00065 if (logstr){ 00066 char myfilestr[100]; 00067 sprintf(myfilestr,"%splanR%d.log",logstr,number); 00068 plannerlog = fopen(myfilestr,"w"); 00069 } 00070 00071 // char mywindowstr[250]; 00072 // if (showPlanner && rbase->getNumber()==0){ 00073 // sprintf(mywindowstr,"plan%d.jpg",number); 00074 // cvNamedWindow(mywindowstr,0); 00075 // } 00076 00077 list<target>::iterator targetListIter; 00078 list<target>::reverse_iterator targetListIterRev; 00079 00080 bool endExploration = false; 00081 int nextPlanning=0; 00082 while(!endPlanner && !endExploration){ 00083 00084 //printID("MARKET BASED PLANNER"); 00085 //Sleep(50); 00086 00087 // esperamos a SLAM 00088 nextStep=mySlam->getStep()+1; 00089 // printf("[MARKETPLANNER] [robot %d] waiting for step %d\n", number, nextStep); 00090 mySlam->waitForStep(nextStep); 00091 00092 if (nextStep > 20000) endExploration = true; 00093 00094 // tomamos los datos 00095 gridMapInterface* omap = mySlam->getOMap(); 00096 binMap* ppmap = mySlam->getPreMap(); 00097 //int numrobots = mySlam->getNumRobots(); 00098 point poscell = mySlam->getCell(number); 00099 pose robotpos = mySlam->getPos(number); 00100 00101 // celda del robot con orientacion 00102 // ocell ori; 00103 // ori.x = poscell.x; 00104 // ori.y = poscell.y; 00105 // float ang = robotpos.th; 00106 // ori.th = int(floor(ang/(PI/4)+0.5)); 00107 // if (ori.th > 7) ori.th -=8; 00108 // if (ori.th < 0) ori.th +=8; 00109 00110 //IplImage* im = (showPlanner && rbase->getNumber()==0)? omap->getMapAsImage():0; 00111 00112 list<auct> auctionList; 00113 list<auct>::iterator aucListIt; 00114 bool newTargets = false; 00115 00116 //******************************************************************************************** >>>>> leer mensajes 00117 string msg; 00118 int msglen; 00119 char sms[100]; 00120 00121 bool listEmptied = false; 00122 00123 auctionList.clear(); 00124 while((msglen=getMessage(msg))){ 00125 // printf("[MARKETPLANNER] [robot %d] --> RECV %s",number,&msg[0]); 00126 StringTokenizer st(&msg[0]); 00127 string messagetype(st.nextToken()); 00128 int robotid = atoi(st.nextToken()); 00129 if (messagetype == "AUCTION"){ // -------->>> NEW AUCTION 00130 // Procesar mensaje 00131 auct aucdest; 00132 aucdest.cell.x = atoi(st.nextToken()); 00133 aucdest.cell.y = atoi(st.nextToken()); 00134 aucdest.auctioner = robotid; 00135 00136 auctionList.push_back(aucdest); // >>> add to the auction list 00137 //printf("[MARKETPLANNER] [robot %d] AUCTION LIST SIZE = %d\n",number,auctionList.size()); 00138 newTargets = true; 00139 } 00140 else if (messagetype == "BID"){ // -------->>> BID RECEIVED 00141 // Procesar mensaje 00142 point aucdest; 00143 aucdest.x = atoi(st.nextToken()); 00144 aucdest.y = atoi(st.nextToken()); 00145 float aucprofit = atof(st.nextToken()); 00146 00147 // mirar si es mejor que las anteriores 00148 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); targetListIter++){ 00149 if (targetListIter->cell.x == aucdest.x && 00150 targetListIter->cell.y == aucdest.y && 00151 targetListIter->bestOfferProfit < aucprofit) 00152 { 00153 targetListIter->bestOfferID = robotid; 00154 targetListIter->bestOfferProfit = aucprofit; // >>> Check if is better 00155 } 00156 } 00157 } 00158 else if (messagetype == "WINNER"){ // -------->>> AUCTION WON 00159 // Procesar mensaje 00160 target t; 00161 int winner = atoi(st.nextToken()); 00162 t.cell.x = atoi(st.nextToken()); 00163 t.cell.y = atoi(st.nextToken()); 00164 t.profit = atof(st.nextToken()); 00165 t.auctionEnds = 0; 00166 00167 if (winner == getDir()){ // ganador, añadir target a la lista si no estaba 00168 bool found = false; 00169 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); targetListIter++){ // si se a reasignado a otro lo borramos de la lista 00170 if (targetListIter->cell.x == t.cell.x && targetListIter->cell.y == t.cell.y){ 00171 found = true; 00172 break; 00173 } 00174 } 00175 if (!found){ 00176 targetList.push_back(t); // >>> add to target list 00177 newTargets=true; 00178 } 00179 } 00180 else{ 00181 if (targetList.size()){ 00182 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); targetListIter++){ // si se a reasignado a otro lo borramos de la lista 00183 if (targetListIter->cell.x == t.cell.x && targetListIter->cell.y == t.cell.y){ 00184 targetListIter = targetList.erase(targetListIter); 00185 targetListIter--; 00186 } 00187 } 00188 if (targetList.size() ==0) listEmptied = true; 00189 } 00190 } 00191 } 00192 else if (messagetype == "END"){ // -------->>> EXPLORATION FINISHED 00193 endExploration = true; 00194 completedPath = true; 00195 } 00196 } 00197 00198 // ************************************************************************************************** si el tiempo del auction expira... 00199 // >>> Enviamos ganador 00200 if (targetList.size()){ 00201 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); ){ 00202 bool erased = false; 00203 if (targetListIter->auctionEnds==1){ // si hay mejores ofertas enviar WINNER 00204 // printf("[MARKETPLANNER] [robot %d] Auction cerrada\n",number); 00205 00206 if (targetListIter->bestOfferID != getDir()){ 00207 sprintf(sms,"WINNER %d %d %d %d %f\n", getDir(), targetListIter->bestOfferID, targetListIter->cell.x, targetListIter->cell.y, targetListIter->bestOfferProfit); 00208 // printf("[MARKETPLANNER] [robot %d] --> SEND TO --> [robotID %d] %s",number,-1,&sms[0]); 00209 sendMessage(string(sms),-1); 00210 // borrar de la lista 00211 targetListIter = targetList.erase(targetListIter); 00212 erased = true; 00213 } 00214 } 00215 if (!erased && targetListIter->auctionEnds>0) targetListIter->auctionEnds--; 00216 if (!erased) ++targetListIter; 00217 } 00218 if (targetList.size() ==0) listEmptied = true; 00219 } 00220 // ************************************************************************************************************ si hemos llegado al ultimo destino planificamos 00221 00222 if(completedPath || newTargets){ 00223 //if(completedPath == true || newTargets || nextStep == nextPlanning){ /// OJO CON ESTO!!! 00224 nextPlanning = nextStep + (int)floor(replanning_period/mySlam->getSampleTime()+0.5); 00225 00226 // printf("[MARKETPLANNER] [robot %d] >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> PLANNING >>>>\n",number); 00227 00228 ///////////////////////////////////// REUNIMOS OBJETIVOS Y CALCULAMOS COSTES //////////////////////////////////// 00229 00230 binMap objectives(omap->getWidth(),omap->getHeight(),omap->getResolution(),omap->getXOrigin(),omap->getYOrigin()); // mapa de objetivos 00231 00232 // cuando hemos completado un destino buscamos más fronteras para añadir 00233 vector<clusterCell> clusterList; 00234 vector<clusterCell>::iterator clusterListIt; 00235 00236 if (targetList.size()==0 || completedPath){ 00237 int nfront = omap->frontiers(objectives); // mapa de fronteras 00238 objectives.cluster(clusterList,2*inflate_obstacles); 00239 objectives.set(clusterList); // fronteras en cluster a objectives list 00240 // printf("[MARKETPLANNER] [robot %d] Frontiers = %d\n",number,clusterList.size()); 00241 } 00242 00243 // los targets de la lista son objetivos tambien al replanificar 00244 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); targetListIter++){ 00245 objectives.set(targetListIter->cell.x,targetListIter->cell.y,true); // añadimos los targets actuales a objectives list 00246 } 00247 00248 // las auctions recibidas tambien son objetivos que hay que analizar 00249 for (aucListIt = auctionList.begin(); aucListIt!= auctionList.end(); aucListIt++){ 00250 objectives.set(aucListIt->cell.x,aucListIt->cell.y,true); // añadimos los auctions a objectives list 00251 } 00252 00253 // Evaluamos el mapa de costes 00254 std::vector<point> targets; 00255 vector<point>::iterator targetIter; 00256 costMapSimple* cm = createCostMap(poscell,objectives,*omap,targets,inflate_obstacles); // crear el costmap costmap 00257 // printf("[MARKETPLANNER] [robot %d] Numero de objetivos distintos: %d\n", number, (int)targets.size()); 00258 00259 ///////////////////////////////////// EVALUAMOS EL BENEFICIO ////////////////////////////////////////// 00260 00261 std::list<target> reachableList; 00262 std::list<target>::iterator reachableListIt; 00263 00264 // evaluamos el beneficio de cada target 00265 point dest = poscell; 00266 int util_cells = ((int)floor(utility_radius/omap->getResolution()+0.5)); 00267 float maxutility = util_cells*util_cells*PI; 00268 float maxcost = (float) cm->getMaxCost(); 00269 for (targetIter = targets.begin(); targetIter!= targets.end(); targetIter++){ 00270 if (targetIter->x != poscell.x && targetIter->y != poscell.y){ 00271 binMap esz; 00272 omap->esz(targetIter->x,targetIter->y,esz,0,util_cells); 00273 00274 target t; 00275 t.utility = utility_weight*utility(*omap,esz)/maxutility; 00276 t.cost = cost_weight*cm->getCost(targetIter->x,targetIter->y)/maxcost; 00277 if (t.cost > 0.0f) 00278 t.profit = t.utility - t.cost; 00279 else 00280 t.profit = -1; 00281 //t.profit = 1 - t.cost; 00282 t.cell = *targetIter; 00283 reachableList.push_back(t); // add evaluated objectives to valid target list 00284 } 00285 } 00286 00287 // printf("[MARKETPLANNER] [robot %d] Targets validos distintos: %d\n",number, (int)reachableList.size()); 00288 // printf("[MARKETPLANNER] [robot %d] Targets en auction list: %d\n",number, (int)auctionList.size()); 00289 // printf("[MARKETPLANNER] [robot %d] Targets en target list: %d\n",number, (int)targetList.size()); 00290 // printf("[MARKETPLANNER] [robot %d] Targets en new frontier cluster list: %d\n",number, (int)clusterList.size()); 00291 // 00292 if (targetList.size()){ 00293 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); targetListIter++){ // borramos los no alcanzables 00294 bool found = false; 00295 for (reachableListIt = reachableList.begin(); reachableListIt!= reachableList.end(); reachableListIt++) 00296 if (targetListIter->cell.x == reachableListIt->cell.x && targetListIter->cell.y == reachableListIt->cell.y) found = true; 00297 if (!found){ 00298 targetList.erase(targetListIter); 00299 targetListIter--; 00300 } 00301 } 00302 if (targetList.size() ==0) listEmptied = true; 00303 } 00304 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00305 //reachableList.sort(); // sort by profit 00306 bool listwasempty = (targetList.size()==0); 00307 for (reachableListIt = reachableList.begin(); reachableListIt!= reachableList.end(); reachableListIt++){ // For each target 00308 // pujamos por cada target de la lista de subastas 00309 for (aucListIt = auctionList.begin(); aucListIt!= auctionList.end(); aucListIt++){ // >> if it comes from the auction list 00310 if (aucListIt->cell.x == reachableListIt->cell.x && 00311 aucListIt->cell.y == reachableListIt->cell.y) 00312 { 00313 sprintf(sms,"BID %d %d %d %f\n", getDir(), aucListIt->cell.x, aucListIt->cell.y, reachableListIt->profit); 00314 // printf("[MARKETPLANNER] [robot %d] --> SEND %s",number,&sms[0]); 00315 sendMessage(string(sms),aucListIt->auctioner); // reply the BID 00316 } 00317 } 00318 // actualizamos los de la lista 00319 if (targetList.size()>0){ 00320 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); ){ // >> if it comes from the target list 00321 bool erased = false; 00322 if (targetListIter->cell.x == reachableListIt->cell.x && 00323 targetListIter->cell.y == reachableListIt->cell.y) 00324 { 00325 if (reachableListIt->utility>0.0011){ // update the target list 00326 //if (omap->isfrontier(reachableListIt->cell.x,reachableListIt->cell.y)) 00327 targetListIter->profit = reachableListIt->profit; 00328 targetListIter->utility = reachableListIt->utility; 00329 targetListIter->cost = reachableListIt->cost; 00330 } 00331 else{ // remove from list if profit < 0 00332 //printf("[MARKETPLANNER] [robot %d] low utility, removing target\n", number); 00333 targetListIter = targetList.erase(targetListIter); 00334 erased = true; 00335 } 00336 } 00337 if (!erased) ++targetListIter; 00338 } 00339 if (targetList.size() ==0){ 00340 listEmptied = true; // HE CAMBIADO ESTO 00341 //printf("[MARKETPLANNER] [robot %d] list is empty\n", number); 00342 } 00343 } 00344 // sacamos a subasta las nuevas fronteras // >> if it comes from the frontier list 00345 for (clusterListIt = clusterList.begin(); clusterListIt!= clusterList.end(); clusterListIt++){ 00346 if (clusterListIt->x == reachableListIt->cell.x && clusterListIt->y == reachableListIt->cell.y 00347 && reachableListIt->utility>0.0011) 00348 { 00349 if (!listwasempty){ // Broadcast Auction 00350 sprintf(sms,"AUCTION %d %d %d %f\n", getDir(), reachableListIt->cell.x, reachableListIt->cell.y, reachableListIt->profit); 00351 // printf("[MARKETPLANNER] [robot %d] --> SEND %s",number,&sms[0]); 00352 sendMessage(string(sms),-1); 00353 bool found = false; 00354 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); targetListIter++){ // si se a reasignado a otro lo borramos de la lista 00355 if (targetListIter->cell.x == reachableListIt->cell.x && targetListIter->cell.y == reachableListIt->cell.y){ 00356 found = true; 00357 break; 00358 } 00359 } 00360 if (!found){ 00361 reachableListIt->auctionEnds = 5; 00362 reachableListIt->bestOfferID=getDir(); 00363 reachableListIt->bestOfferProfit=reachableListIt->profit; 00364 targetList.push_back(*reachableListIt); // lo guardamos en el target list 00365 } 00366 else{ 00367 targetListIter->auctionEnds = 5; 00368 targetListIter->bestOfferID=getDir(); 00369 targetListIter->bestOfferProfit=reachableListIt->profit; 00370 } 00371 } 00372 else{ 00373 reachableListIt->auctionEnds = 0; 00374 targetList.push_back(*reachableListIt); 00375 } 00376 break; 00377 } 00378 } 00379 } 00380 00381 // printf("[MARKETPLANNER] [robot %d] targetList size after replanning = %d\n",number, targetList.size()); 00382 00383 // ordenamos los targets de la lista y elegimos el mejor 00384 if (targetList.size()){ 00385 targetList.sort(); 00386 //for (targetListIterRev = targetList.rbegin(); targetListIterRev != targetList.rend(); targetListIterRev++ ){ 00387 //if (targetListIterRev->auctionEnds==0){ 00388 //dest = targetListIterRev->cell; 00389 //break; 00390 //} 00391 //} 00392 dest = targetList.back().cell; 00393 cm->getPath(dest,path); 00394 } 00395 else if(!listEmptied){ // HE CAMBIADO ESTO 00396 // printf("[MARKETPLANNER] [robot %d] No path to frontiers, stopping exploration\n",number); 00397 endExploration = true; 00398 completedPath = true; 00399 char sms[100]; 00400 sprintf(sms,"END %d", getDir()); 00401 sendMessage(string(sms),-1); 00402 } 00403 delete cm; 00404 completedPath = false; 00405 newTargets = false; 00406 // printf("[MARKETPLANNER] [robot %d] >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> PLANNING END >>>>\n",number); 00407 // printf("[MARKETPLANNER] [robot %d] destination: %d, %d, path ini (%d,%d) \n",number, dest.x,dest.y, path.back().x, path.back().y); 00408 } 00409 00410 // Seguimos con la ruta 00411 else { 00412 // quitamos el punto de la lista si hemos llegado 00413 for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); targetListIter++){ 00414 if (abs(targetListIter->cell.x-poscell.x) <= 1 && abs(targetListIter->cell.y-poscell.y) <=1){ 00415 targetListIter = targetList.erase(targetListIter); 00416 break; 00417 } 00418 } 00419 00420 // verificamos que el path sea transitable 00421 bool obstructedPath = false; 00422 vector<point>::reverse_iterator riter; 00423 if(path.size()>1) { 00424 for ( riter = path.rbegin(); riter != path.rend(); riter++ ) 00425 if (omap->isoccupied(riter->x,riter->y)) { 00426 // printf("[MARKETPLANNER] [robot %d] obstructed\n",rbase->getNumber()); 00427 obstructedPath = true; 00428 break; 00429 } 00430 } 00431 // verificar distancia al destino || obstruccion para path completado 00432 if((sqrt(pow((float)(path.front().x-poscell.x),2) + pow((float)(path.front().y-poscell.y),2)) <= 3.0) || obstructedPath){ 00433 // printf("[MARKETPLANNER] [robot %d] completed\n",rbase->getNumber()); 00434 completedPath = true; 00435 } 00436 else { // No hemos llegado al final seguimos avanzando 00437 bool found=false; 00438 00439 //IplImage* im = (showPlanner)? omap->getMapAsImage() : 0; 00440 //if (showPlanner){ 00441 // cvCircle(im,cvPoint(poscell.x, omap->getHeight()-1-poscell.y),0,CV_RGB(255,0,0),-1); //posicion rojo 00442 // for (unsigned int i = 0;i < path.size(); i++) 00443 // cvCircle(im,cvPoint(path[i].x, omap->getHeight()-1-path[i].y),0,CV_RGB(255,0,255),-1); // path magenta 00444 // cvShowImage(mywindowstr,im); 00445 //} 00446 00447 if(path.size()>=1) { 00448 for ( riter = path.rbegin(); riter != path.rend(); riter++ ){ 00449 bool visible = true; 00450 int size; 00451 point* line = omap->getLine(poscell.x,poscell.y,riter->x,riter->y,size); 00452 00453 for (int p = 0; p < size; p++){ 00454 if (p > 1 && omap->isoccupied(line[p].x,line[p].y,1)){ 00455 visible = false; 00456 break; 00457 } 00458 // else 00459 // cvCircle(im,cvPoint(line[p].x, omap->getHeight()-1-line[p].y),0,CV_RGB(0,255,0),-1); // visual green 00460 } 00461 //cvShowImage(mywindowstr,im); 00462 //cvWaitKey(2); 00463 delete [] line; 00464 if (visible) { // is visible 00465 goal.x = riter->x; 00466 goal.y = riter->y; 00467 if (found) path.pop_back(); 00468 else found = true; 00469 //path.pop_back(); 00470 double dist = sqrt(pow(poscell.x-goal.x,2) + pow(poscell.y-goal.y,2)); 00471 if ( dist > 1.5/omap->getResolution()) break; 00472 } 00473 else break; 00474 } 00475 } 00476 if(!found){ 00477 completedPath = true; // algo falla ninguna celda de la ruta es visible 00478 // printf("[MARKETPLANNER] [robot %d] no visible cells\n",rbase->getNumber()); 00479 } 00480 //if (showPlanner) cvReleaseImage(&im); 00481 00482 } 00483 // printf("[MARKETPLANNER] [robot %d] next goal: (%d,%d) - current pos (%d,%d)\n",rbase->getNumber(),goal.x, goal.y, ori.x,ori.y); 00484 } 00485 00486 // visualizacion 00487 // if (showPlanner && rbase->getNumber()==0){ 00488 // cvCircle(im,cvPoint(poscell.x, omap->getHeight()-1-poscell.y),0,CV_RGB(255,0,0),-1); //posicion rojo 00489 // cvCircle(im,cvPoint(poscell.x+avancex[ori.th], omap->getHeight()-1-poscell.y-avancey[ori.th]),0,CV_RGB(0,0,255),-1); // orientacion azul 00490 // for (unsigned int i = 0;i < path.size(); i++) 00491 // cvCircle(im,cvPoint(path[i].x, omap->getHeight()-1-path[i].y),0,CV_RGB(255,0,255),-1); // path magenta 00492 // if (targetList.size()>0) 00493 // for (targetListIter = targetList.begin(); targetListIter!= targetList.end(); targetListIter++ ) 00494 // if (targetListIter->auctionEnds==0) 00495 // cvCircle(im,cvPoint(targetListIter->cell.x , omap->getHeight()-1-targetListIter->cell.y),1,CV_RGB(255,0,0),-1); // targets red 00496 // else 00497 // cvCircle(im,cvPoint(targetListIter->cell.x , omap->getHeight()-1-targetListIter->cell.y),1,CV_RGB(0,255,0),-1); // targets red 00498 // cvShowImage(mywindowstr,im); 00499 // cvWaitKey(5); 00500 // cvReleaseImage(&im); 00501 // } 00502 00503 delete omap; 00504 delete ppmap; 00505 00506 // modificar los enables/goal de la parte reactiva 00507 if (completedPath) reac->disableAll(); // paramos el robot hasta planificar de nuevo 00508 else{ 00509 // printf("\t\t\t\t\t[robot %d] ************************************** Following Path\n",number); 00510 00511 reac->setGoal(goal); 00512 reac->enableGoToGoal(); 00513 reac->enableAvoidObstacles(); 00514 reac->disableAvoidOtherRobots(); 00515 reac->disableGoToFrontier(); 00516 reac->disableGoToUnexploredZones(); 00517 reac->disableGoToPrecisePoses(); 00518 } 00519 00520 // write logs 00521 if (logstr) fprintf(plannerlog,"%d, %d, %d, %d\n", nextStep, completedPath, poscell.x, poscell.y); 00522 } 00523 00524 // if (showPlanner && rbase->getNumber()==0) cvDestroyWindow(mywindowstr); 00525 00526 //closing logs 00527 if (logstr) fclose(plannerlog); 00528 00529 // printf("[robot %d] stoping robot", number); 00530 reac->disableAll(); 00531 reac->stop(); 00532 00533 closing.unlock(); 00534 explorationFinished.step(); 00535 } 00536 00537 int MarketPlanner::utility(const gridMapInterface& omap, const binMap& esz){ 00538 int value=0; 00539 for (int i = esz.getRoi().x ; i < esz.getRoi().x + esz.getRoi().width ; i++){ 00540 for (int j = esz.getRoi().y ; j < esz.getRoi().y + esz.getRoi().height ; j++){ // for each cell 00541 if (esz.get(i,j) && omap.isunknown(i,j)){ 00542 value++; 00543 } 00544 } 00545 } 00546 return value; 00547 }
1.7.6.1
