|
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 "IntegratedIGPlanner.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 #include "matFuns.h" 00012 #include "robotTypes.h" 00013 00014 // Para registrar la clase en la factoria 00015 namespace planners{ 00016 planner* CreateIntegratedIGPlanner(const ConfigFile& conf){ 00017 return new IntegratedIGPlanner(conf); 00018 }; 00019 const int INTEGRATEDIGPLANNER = 6 ; 00020 const bool registered = plannerFactory::Instance().Register(INTEGRATEDIGPLANNER, plannerCreator(CreateIntegratedIGPlanner)); 00021 } 00022 00023 using namespace std; 00024 00025 // Constructor 00026 IntegratedIGPlanner::IntegratedIGPlanner(const ConfigFile& config): 00027 planner(), 00028 //policy(0), 00029 endPlanner(true), 00030 replanning_period(config.read<float>("REPLANNING_PERIOD")), 00031 inflate_obstacles(config.read<int>("INFLATE_OBSTACLES")), 00032 utility_radius(config.read<float>("UTILITY_RADIUS")), 00033 utility_weight(config.read<float>("UTILITY_WEIGHT")), 00034 cost_weight(config.read<float>("COST_WEIGHT")), 00035 localization_weight(config.read<float>("LOCALIZATION_WEIGHT")), 00036 camrange(config.read<float>("CAMERA_RANGE")) 00037 {} 00038 00039 // Destructor 00040 IntegratedIGPlanner::~IntegratedIGPlanner(){stop();} 00041 00042 // thread Setup 00043 int IntegratedIGPlanner::setup(){ 00044 00045 // printf("arranco planificador\n"); 00046 completedPath = true; 00047 prio = 50; 00048 return 0; 00049 } 00050 00051 // thread on stop 00052 void IntegratedIGPlanner::onStop(){ 00053 if (!endPlanner){ 00054 endPlanner = true; 00055 closing.lock(); 00056 closing.unlock(); 00057 } 00058 printf("[INTEGRATED_PLANNER] [robot %d] Stopped\n", rbase->getNumber()); 00059 } 00060 00061 // thread main execution 00062 void IntegratedIGPlanner::execute(){ 00063 printf("[INTEGRATED_PLANNER] \t\t\t\t\t INTEGRATED_PLANNER (%d) running\n", rbase->getNumber()); 00064 closing.lock(); 00065 endPlanner = false; 00066 point goal; 00067 int nextStep=number; 00068 00069 FILE* plannerlog=0; 00070 if (logstr){ 00071 char myfilestr[100]; 00072 sprintf(myfilestr,"%splanR%d.log",logstr,number); 00073 plannerlog = fopen(myfilestr,"w"); 00074 } 00075 00076 // char mywindowstr[250]; 00077 // if (showPlanner){ 00078 // sprintf(mywindowstr,"plan%d.jpg",number); 00079 // cvNamedWindow(mywindowstr,0); 00080 // } 00081 00082 bool endExploration = false; 00083 int nextPlanning=0; 00084 while(!endPlanner && !endExploration){ 00085 00086 // esperamos a SLAM 00087 nextStep=mySlam->getStep()+1; 00088 // printf("[INTEGRATEDIGPLANNER] [robot %d planner] waiting for step %d\n", number, nextStep); 00089 mySlam->waitForStep(nextStep); 00090 00091 if (nextStep > 20000) endExploration = true; 00092 00093 // tomamos los datos 00094 gridMapInterface* omap = mySlam->getOMap(); 00095 binMap* ppmap = mySlam->getPreMap(); 00096 //int numrobots = mySlam->getNumRobots(); 00097 point poscell = mySlam->getCell(number); 00098 pose robotpos = mySlam->getPos(number); 00099 visualMap* vmap = mySlam->getVMap(); 00100 00101 // celda 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 // leer mensajes 00113 string msg; 00114 int msglen; 00115 while((msglen=getMessage(msg))){ 00116 //cout << "Message: >> " << msg << endl; 00117 StringTokenizer st(&msg[0]); 00118 string messagetype(st.nextToken()); 00119 if (messagetype == "END"){ 00120 endExploration = true; 00121 completedPath = true; 00122 } 00123 } 00124 00125 // si hemos llegado al ultimo destino planificamos 00126 if(completedPath == true || nextStep == nextPlanning){ 00127 nextPlanning = nextStep + (int)floor(replanning_period/mySlam->getSampleTime()+0.5); 00128 00129 completedPath = false; 00130 // printf("[INTEGRATEDIGPLANNER] >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> PLANNING >>>> robot %d\n",number); 00131 // mapa de fronteras 00132 binMap objectives; 00133 vector<clusterCell> clusterList; 00134 omap->frontiers(objectives); 00135 objectives.cluster(clusterList,2*inflate_obstacles); 00136 objectives.set(clusterList); //objectives.showMap("FRONTERAS"); 00137 00138 // mapa de costes 00139 vector<point> targets; 00140 vector<point>::iterator targetIter; 00141 costMapSimple* cm = createCostMap(poscell,objectives,*omap,targets,inflate_obstacles); 00142 00143 // printf("[INTEGRATEDIGPLANNER] Numero de objetivos: %d\n",(int)targets.size()); 00144 00145 // elegir destino 00146 point dest = poscell; 00147 float maxVal = -9999999999999.9f; 00148 int util_cells = ((int)floor(utility_radius/omap->getResolution()+0.5)); 00149 float maxutility = util_cells*util_cells*PI; 00150 float maxcost = cm->getMaxCost(); 00151 float det = mySlam->getCovariance(number).det(); 00152 float maxlocutil = -10000.0f; 00153 if (det > 0) maxlocutil= -0.5*log( pow(PIx2*exp(1.0f),3) * det); 00154 00155 // printf("[INTEGRATEDIGPLANNER] decidiendo el destino....\n"); 00156 for (targetIter = targets.begin(); targetIter!= targets.end(); targetIter++){ 00157 // if (fabs(targetIter->x-ori.x) + fabs(targetIter->y - ori.y) > 15){ 00158 00159 binMap esz; 00160 omap->esz(targetIter->x,targetIter->y,esz,0,util_cells); 00161 float utilvalue = IGutility(*omap,esz); 00162 float cost = cm->getCost(targetIter->x,targetIter->y); 00163 esz.dilate(2); 00164 float locvalue = LOCutility(*targetIter,esz,*vmap,mySlam->getGlobalCovariance()); 00165 // printf("loc= %f(%f), cost= %f(%f), util= %f(%f)\n",locvalue,maxlocutil,cost,maxcost,utilvalue,maxutility); 00166 float value = utility_weight*utilvalue/maxutility - cost_weight*cost/maxcost + localization_weight*locvalue/maxlocutil; 00167 //printf("(%d,%d) IGutility = %f, localizability = %f, cost= %f \n",targetIter->x,targetIter->y,utilvalue,locvalue, cost); 00168 if (value > maxVal){ 00169 maxVal = value; 00170 dest = *targetIter; 00171 } 00172 // } 00173 } 00174 // printf("[INTEGRATEDIGPLANNER] decidido\n"); 00175 00176 cm->getPath(dest,path); 00177 00178 // printf("[INTEGRATEDIGPLANNER] length of the path = %d\n",path.size()); 00179 delete cm; 00180 00181 if (targets.size()==0){ 00182 // printf("[INTEGRATEDIGPLANNER] No path to frontiers, stopping exploration\n"); 00183 endPlanner = true; 00184 completedPath = true; 00185 char sms[100]; 00186 sprintf(sms,"END %d", getDir()); 00187 sendMessage(string(sms),-1); 00188 } 00189 // printf("[INTEGRATEDIGPLANNER] >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> PLANNING END >>>> robot %d\n",number); 00190 } 00191 00192 // Seguimos con la ruta 00193 else { 00194 00195 // verificamos que el path sea transitable 00196 bool obstructedPath = false; 00197 vector<point>::reverse_iterator riter; 00198 if(path.size()>1) { 00199 for ( riter = path.rbegin(); riter != path.rend(); riter++ ) 00200 if (omap->isoccupied(riter->x,riter->y)) { 00201 // printf("[INTEGRATEDIGPLANNER] [robot %d] obstructed\n",rbase->getNumber()); 00202 obstructedPath = true; 00203 break; 00204 } 00205 } 00206 // verificar distancia al destino || obstruccion para path completado 00207 if((sqrt(pow((float)(path.front().x-poscell.x),2) + pow((float)(path.front().y-poscell.y),2)) <= 3.0) || obstructedPath){ 00208 // printf("[INTEGRATEDIGPLANNER] [robot %d] completed\n",rbase->getNumber()); 00209 completedPath = true; 00210 } 00211 else { // No hemos llegado al final seguimos avanzando 00212 bool found=false; 00213 00214 //IplImage* im = (showPlanner)? omap->getMapAsImage() : 0; 00215 //if (showPlanner){ 00216 // cvCircle(im,cvPoint(poscell.x, omap->getHeight()-1-poscell.y),0,CV_RGB(255,0,0),-1); //posicion rojo 00217 // for (unsigned int i = 0;i < path.size(); i++) 00218 // cvCircle(im,cvPoint(path[i].x, omap->getHeight()-1-path[i].y),0,CV_RGB(255,0,255),-1); // path magenta 00219 // cvShowImage(mywindowstr,im); 00220 //} 00221 00222 if(path.size()>=1) { 00223 for ( riter = path.rbegin(); riter != path.rend(); riter++ ){ 00224 bool visible = true; 00225 int size; 00226 point* line = omap->getLine(poscell.x,poscell.y,riter->x,riter->y,size); 00227 00228 for (int p = 0; p < size; p++){ 00229 if (p > 1 && omap->isoccupied(line[p].x,line[p].y,1)){ 00230 visible = false; 00231 break; 00232 } 00233 // else 00234 // cvCircle(im,cvPoint(line[p].x, omap->getHeight()-1-line[p].y),0,CV_RGB(0,255,0),-1); // visual green 00235 } 00236 //cvShowImage(mywindowstr,im); 00237 //cvWaitKey(2); 00238 delete [] line; 00239 if (visible) { // is visible 00240 goal.x = riter->x; 00241 goal.y = riter->y; 00242 if (found) path.pop_back(); 00243 else found = true; 00244 //path.pop_back(); 00245 double dist = sqrt(pow(poscell.x-goal.x,2) + pow(poscell.y-goal.y,2)); 00246 if ( dist > 1.5/omap->getResolution()) break; 00247 } 00248 else break; 00249 } 00250 } 00251 if(!found){ 00252 completedPath = true; // algo falla ninguna celda de la ruta es visible 00253 // printf("[INTEGRATEDIGPLANNER] [robot %d] no visible cells\n",rbase->getNumber()); 00254 } 00255 //if (showPlanner) cvReleaseImage(&im); 00256 00257 } 00258 // printf("[INTEGRATEDIGPLANNER] [robot %d] next goal: (%d,%d) - current pos (%d,%d)\n",rbase->getNumber(),goal.x, goal.y, ori.x,ori.y); 00259 } 00260 00261 // visualizacion 00262 // if (showPlanner){ 00263 // cvCircle(im,cvPoint(poscell.x, omap->getHeight()-1-poscell.y),0,CV_RGB(255,0,0),-1); //posicion rojo 00264 // 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 00265 // for (unsigned int i = 0;i < path.size(); i++) 00266 // cvCircle(im,cvPoint(path[i].x, omap->getHeight()-1-path[i].y),0,CV_RGB(255,0,255),-1); // path magenta 00267 // cvShowImage(mywindowstr,im); 00268 // cvWaitKey(5); 00269 // cvReleaseImage(&im); 00270 // } 00271 00272 00273 delete omap; 00274 delete ppmap; 00275 delete vmap; 00276 00277 // modificar los enables/goal de la parte reactiva 00278 if (completedPath) reac->disableAll(); // paramos el robot hasta planificar de nuevo 00279 else{ 00280 // printf("************************************** [robot %d] Following Path\n",number); 00281 00282 00283 reac->setGoal(goal); 00284 reac->enableGoToGoal(); 00285 reac->enableAvoidObstacles(); 00286 reac->disableAvoidOtherRobots(); 00287 reac->disableGoToFrontier(); 00288 reac->disableGoToUnexploredZones(); 00289 reac->disableGoToPrecisePoses(); 00290 } 00291 00292 // write logs 00293 if (logstr) fprintf(plannerlog,"%d, %d, %d, %d\n", nextStep, completedPath, poscell.x, poscell.y); 00294 } 00295 00296 // if (showPlanner) cvDestroyWindow(mywindowstr); 00297 00298 //closing logs 00299 if (logstr) fclose(plannerlog); 00300 00301 // printf("[robot %d] stoping robot", number); 00302 reac->disableAll(); 00303 reac->stop(); 00304 00305 closing.unlock(); 00306 explorationFinished.step(); 00307 } 00308 00309 00310 00311 float IntegratedIGPlanner::IGutility(const gridMapInterface& omap, const binMap& esz){ 00312 float entropy=0; 00313 for (int i = esz.getRoi().x ; i < esz.getRoi().x + esz.getRoi().width ; i++){ 00314 for (int j = esz.getRoi().y ; j < esz.getRoi().y + esz.getRoi().height ; j++){ // for each cell 00315 if (esz.get(i,j)){ 00316 float pocc = omap.getValue(i,j)/100.0f; 00317 float pemp = 1-pocc; 00318 if (pocc > 0 && pemp > 0) 00319 entropy += pocc*log(pocc)+pemp*log(pemp); 00320 } 00321 } 00322 } 00323 return -entropy; 00324 } 00325 00326 float IntegratedIGPlanner::LOCutility(const point& oc, const binMap esz, visualMap& vmap, const Ematrix& P){ 00327 float camrange2 = camrange*camrange; 00328 int nbots = mySlam->getNumRobots(); 00329 00330 pose pos; 00331 pos.x = mySlam->getXOrigin() + (oc.x+0.5)*mySlam->getResolution(); 00332 pos.y = mySlam->getYOrigin() + (oc.y+0.5)*mySlam->getResolution(); 00333 pos.th = 0; //oc.th*PI/4.0f; 00334 00335 Ematrix Hm(0,0,3*vmap.getNLandmarks(),3*vmap.getNLandmarks()); 00336 Ematrix Hv(0,3,3*vmap.getNLandmarks(),3); 00337 00338 int visiblemarks=0; 00339 int* marksnumbers = new int[vmap.getNLandmarks()]; 00340 00341 Ematrix Hmi = jacobianHm(pos); 00342 00343 for (int l = 0; l < vmap.getNLandmarks(); l++){ 00344 landmark* mark = vmap.getLandmarkById(l); 00345 float dist2 = pow(mark->pos.getX()-pos.x,2)+pow(mark->pos.getY()-pos.y,2); 00346 if(dist2 < camrange2){ 00347 Hm.extend(3,3); 00348 Hm.set(3*visiblemarks,3*visiblemarks,Hmi); 00349 Hv.extend(3,0); 00350 Hv.set(3*visiblemarks,0,jacobianHv(pos,mark->pos)); 00351 marksnumbers[visiblemarks] = l; 00352 visiblemarks++; 00353 } 00354 delete mark; 00355 } 00356 00357 Ematrix Hmtrans = Hm.transpose(); 00358 Ematrix Hvtrans = Hv.transpose(); 00359 00360 Ematrix Pom(3*visiblemarks,3*visiblemarks); 00361 for (int l1 = 0; l1 < visiblemarks; l1++){ 00362 for (int l2 = 0; l2 < visiblemarks; l2++){ 00363 Pom.set(l1*3,l2*3,P.subMat(3*nbots+3*marksnumbers[l1],3*nbots+3*marksnumbers[l1]+3, 00364 3*nbots+3*marksnumbers[l2],3*nbots+3*marksnumbers[l2]+3)); 00365 } 00366 } 00367 00368 delete[] marksnumbers; 00369 //printf("visiblemarks=%d\n",visiblemarks); 00370 if (visiblemarks > 0){ 00371 Ematrix Pvvinf = (Hvtrans*((Hm*(Pom*Hmtrans)).inverse()*Hv)).inverse(); 00372 //P.print("P="); 00373 //Pom.print("Pom="); 00374 //Hm.print("Hm="); 00375 //Hv.print("Hv="); 00376 //Pvvinf.print("Pvvinf="); 00377 float det = Pvvinf.det(); 00378 float locutil = -10000.0f; 00379 if (det > 0) locutil= -0.5*log( pow(PIx2*exp(1.0f),3) * det); 00380 //printf("det= %e, locutil = %f\n",det,locutil); 00381 return locutil; 00382 } 00383 else return 0.0f; 00384 } 00385 00386 // jacobian H 00387 matrix IntegratedIGPlanner::jacobianHv(const pose& pos, const pos3d& mark){ 00388 matrix H(3,3); 00389 float costh = (float)cos(pos.th); 00390 float sinth = (float)sin(pos.th); 00391 float auxx = mark.getX() - pos.x; 00392 float auxy = mark.getY() - pos.y; 00393 H.set(0,0,-costh); 00394 H.set(0,1,-sinth); 00395 H.set(0,2,-auxx*sinth + auxy*costh ); 00396 H.set(1,0, sinth); 00397 H.set(1,1,-costh); 00398 H.set(1,2,-auxx*costh - auxy*sinth ); 00399 return H; 00400 } 00401 00402 matrix IntegratedIGPlanner::jacobianHm(const pose& pos){ 00403 matrix H(3,3); 00404 float costh = (float)cos(pos.th); 00405 float sinth = (float)sin(pos.th); 00406 H.set(0,0,costh); 00407 H.set(0,1,sinth); 00408 H.set(1,0,-sinth); 00409 H.set(1,1,costh); 00410 H.set(2,2,1.0f); 00411 return H; 00412 }
1.7.6.1
