|
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 "FrontierGreedyPlanner.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 #define DEBUG false 00013 00014 // Para registrar la clase en la factoria 00015 namespace planners{ 00016 planner* CreateFrontierGreedyPlanner(const ConfigFile& conf){ 00017 return new FrontierGreedyPlanner(conf); 00018 }; 00019 const int FRONTIERGREEDYPLANNER = 2 ; 00020 const bool registered = plannerFactory::Instance().Register(FRONTIERGREEDYPLANNER, plannerCreator(CreateFrontierGreedyPlanner)); 00021 } 00022 00023 using namespace std; 00024 00025 // Constructor 00026 FrontierGreedyPlanner::FrontierGreedyPlanner(const ConfigFile& config): 00027 planner(), 00028 replanning_period(config.read<float>("REPLANNING_PERIOD")), 00029 inflate_obstacles(config.read<int>("INFLATE_OBSTACLES")), 00030 endPlanner(true) 00031 {} 00032 00033 // Destructor 00034 FrontierGreedyPlanner::~FrontierGreedyPlanner(){stop();} 00035 00036 // thread Setup 00037 int FrontierGreedyPlanner::setup(){ 00038 completedPath = true; 00039 prio = 50; 00040 return 0; 00041 } 00042 00043 // thread on stop 00044 void FrontierGreedyPlanner::onStop(){ 00045 if (!endPlanner){ 00046 endPlanner = true; 00047 closing.lock(); 00048 closing.unlock(); 00049 } 00050 printf("[GREEDYPLANNER] [robot %d] Stopped\n", rbase->getNumber()); 00051 } 00052 00053 // thread main execution 00054 void FrontierGreedyPlanner::execute(){ 00055 printf("[GREEDYPLANNER] \t\t\t\t\t GREEDYPLANNER (%d) running\n", rbase->getNumber()); 00056 00057 closing.lock(); 00058 endPlanner = false; 00059 point goal; 00060 int nextStep=number; 00061 00062 FILE* plannerlog=0; 00063 if (logstr){ 00064 char myfilestr[100]; 00065 sprintf(myfilestr,"%splanR%d.log",logstr,number); 00066 plannerlog = fopen(myfilestr,"w"); 00067 } 00068 00069 // char mywindowstr[250]; 00070 // if (showPlanner){ 00071 // sprintf(mywindowstr,"plan%d.jpg",number); 00072 // cvNamedWindow(mywindowstr,0); 00073 // } 00074 00075 bool endExploration = false; 00076 int nextPlanning=0; 00077 00078 while(!endPlanner && !endExploration){ 00079 00080 // esperamos a SLAM 00081 nextStep=mySlam->getStep()+1; 00082 //if (DEBUG) printf("[GREEDYPLANNER] [robot %d] waiting for step %d\n", rbase->getNumber(), nextStep); 00083 mySlam->waitForStep(nextStep); 00084 00085 if (nextStep > 20000) endExploration = true; 00086 00087 // tomamos los datos 00088 gridMapInterface* omap = mySlam->getOMap(); 00089 binMap* ppmap = mySlam->getPreMap(); 00090 // int numrobots = mySlam->getNumRobots(); 00091 point poscell = mySlam->getCell(number); 00092 pose robotpos = mySlam->getPos(number); 00093 00094 // celda con orientacion 00095 // ocell ori; 00096 // ori.x = poscell.x; 00097 // ori.y = poscell.y; 00098 // float ang = robotpos.th; 00099 // ori.th = int(floor(ang/(PI/4)+0.5)); 00100 // if (ori.th > 7) ori.th -=8; 00101 // if (ori.th < 0) ori.th +=8; 00102 00103 // leer mensajes 00104 string msg; 00105 int msglen; 00106 while((msglen=getMessage(msg))){ 00107 //cout << "Message: >> " << msg << endl; 00108 StringTokenizer st(&msg[0]); 00109 string messagetype(st.nextToken()); 00110 if (messagetype == "END"){ 00111 endExploration = true; 00112 completedPath = true; 00113 } 00114 } 00115 00116 // IplImage* im = (showPlanner)? omap->getMapAsImage():0; 00117 00118 // si hemos llegado al ultimo destino planificamos 00119 if(completedPath == true || nextStep >= nextPlanning){ 00120 nextPlanning = nextStep + (int)floor(replanning_period/mySlam->getSampleTime()+0.5); 00121 completedPath = false; 00122 00123 binMap objectives; 00124 vector<clusterCell> clusterList; 00125 omap->frontiers(objectives); 00126 objectives.cluster(clusterList,2*inflate_obstacles); 00127 objectives.set(clusterList); 00128 00129 // printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> DIJKSTRA >>>> robot %d\n",number); 00130 // int lenpath = DijkstraGridWithHeading(ori, objectives,*omap,path); 00131 int lenpath = DijkstraGrid(poscell, objectives,*omap,path, inflate_obstacles); // ojo!!! la lista va del destino al origen!!! 00132 // for ( int j = 0; j < lenpath; j++) if (DEBUG) printf("(%d,%d), ",path[j].x, path[j].y); 00133 // if (DEBUG) printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> FIN DE LA PLANIFICACION: length: %d >>>> robot %d\n",lenpath,number); 00134 // if (DEBUG) printf("[GREEDYPLANNER] [robot %d] >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> FIN DE LA PLANIFICACION: length: %d \n",rbase->getNumber(),lenpath); 00135 00136 if (lenpath == 0){ 00137 // if (DEBUG) printf("No path to frontiers, stopping exploration\n"); 00138 endExploration = true; 00139 completedPath = true; 00140 char sms[100]; 00141 sprintf(sms,"END %d", getDir()); 00142 sendMessage(string(sms),-1); 00143 } 00144 } 00145 // Seguimos con la ruta 00146 else { 00147 00148 // verificamos que el path sea transitable 00149 bool obstructedPath = false; 00150 vector<point>::reverse_iterator riter; 00151 if(path.size()>1) { 00152 for ( riter = path.rbegin(); riter != path.rend(); riter++ ) 00153 if (omap->isoccupied(riter->x,riter->y)) { 00154 // if (DEBUG) printf("[GREEDYPLANNER] [robot %d] obstructed\n",rbase->getNumber()); 00155 obstructedPath = true; 00156 break; 00157 } 00158 } 00159 // verificar distancia al destino || obstruccion para path completado 00160 if((sqrt(pow((float)(path.front().x-poscell.x),2) + pow((float)(path.front().y-poscell.y),2)) <= 3.0) || obstructedPath){ 00161 // if (DEBUG) printf("[GREEDYPLANNER] [robot %d] completed\n",rbase->getNumber()); 00162 completedPath = true; 00163 } 00164 else { // No hemos llegado al final seguimos avanzando 00165 bool found=false; 00166 00167 //IplImage* im = (showPlanner)? omap->getMapAsImage() : 0; 00168 //if (showPlanner){ 00169 // cvCircle(im,cvPoint(poscell.x, omap->getHeight()-1-poscell.y),0,CV_RGB(255,0,0),-1); //posicion rojo 00170 // for (unsigned int i = 0;i < path.size(); i++) 00171 // cvCircle(im,cvPoint(path[i].x, omap->getHeight()-1-path[i].y),0,CV_RGB(255,0,255),-1); // path magenta 00172 // cvShowImage(mywindowstr,im); 00173 //} 00174 00175 if(path.size()>=1) { 00176 for ( riter = path.rbegin(); riter != path.rend(); riter++ ){ 00177 bool visible = true; 00178 int size; 00179 point* line = omap->getLine(poscell.x,poscell.y,riter->x,riter->y,size); 00180 00181 for (int p = 0; p < size; p++){ 00182 if (p > 1 && omap->isoccupied(line[p].x,line[p].y,1)){ 00183 visible = false; 00184 break; 00185 } 00186 // else 00187 // cvCircle(im,cvPoint(line[p].x, omap->getHeight()-1-line[p].y),0,CV_RGB(0,255,0),-1); // visual green 00188 } 00189 //cvShowImage(mywindowstr,im); 00190 //cvWaitKey(2); 00191 delete [] line; 00192 if (visible) { // is visible 00193 goal.x = riter->x; 00194 goal.y = riter->y; 00195 if (found) path.pop_back(); 00196 else found = true; 00197 //path.pop_back(); 00198 double dist = sqrt(pow(poscell.x-goal.x,2) + pow(poscell.y-goal.y,2)); 00199 if ( dist > 1.5/omap->getResolution()) break; 00200 } 00201 else break; 00202 } 00203 } 00204 if(!found){ 00205 completedPath = true; // algo falla ninguna celda de la ruta es visible 00206 // if (DEBUG) printf("[GREEDYPLANNER] [robot %d] no visible cells\n",rbase->getNumber()); 00207 } 00208 //if (showPlanner) cvReleaseImage(&im); 00209 00210 } 00211 // if (DEBUG) printf("[GREEDYPLANNER] [robot %d] next goal: (%d,%d) - current pos (%d,%d)\n",rbase->getNumber(),goal.x, goal.y, ori.x,ori.y); 00212 } 00213 00214 00215 /* 00216 // Quitamos los puntos que hayamos recorrido 00217 else { 00218 // verificamos que el path sea transitable 00219 bool obstructedPath = false; 00220 //vector<ocell>::iterator iter; 00221 vector<point>::iterator iter; 00222 if(path.size()>1) { 00223 for ( iter = path.end()-1; iter != path.begin(); iter-- ) 00224 if (omap->isoccupied(iter->x,iter->y)) { 00225 if (DEBUG) printf("[GREEDYPLANNER] [robot %d] obstructed\n",rbase->getNumber()); 00226 obstructedPath = true; 00227 break; 00228 } 00229 } 00230 // verificar distancia al destino || obstruccion para path completado 00231 if((sqrt(pow((float)(path.front().x-poscell.x),2) + pow((float)(path.front().y-poscell.y),2)) < 3.0) || obstructedPath){ 00232 if (DEBUG) printf("[GREEDYPLANNER] [robot %d] completed\n",rbase->getNumber()); 00233 completedPath = true; 00234 } 00235 else { 00236 bool found=false; 00237 if(path.size()>1 && vmax>0) { 00238 for ( iter = path.end()-1; iter != path.begin(); iter-- ){ 00239 bool visible = true; 00240 int size,p; 00241 point* line = omap->getLine(iter->x,iter->y,poscell.x,poscell.y,size); 00242 for (p = 0; p < size; p++){ 00243 if (omap->isoccupied(line[p].x,line[p].y,2)){ visible = false; break;} 00244 } 00245 delete [] line; 00246 if (visible) { // is visible 00247 goal.x = iter->x; 00248 goal.y = iter->y; 00249 found = true; 00250 int dist = fabs(poscell.x-goal.x) + fabs(poscell.y-goal.y); 00251 if ( dist < 1.0/omap->getResolution()) { 00252 path.erase(iter); 00253 iter--; 00254 } 00255 if ( dist > 10.0/omap->getResolution()) break; 00256 } 00257 else break; 00258 } 00259 } 00260 if(!found){ 00261 completedPath = true; // algo falla ninguna celda de la ruta es visible 00262 if (DEBUG) printf("[GREEDYPLANNER] [robot %d] no visible cells\n",rbase->getNumber()); 00263 } 00264 } 00265 if (DEBUG) printf("[GREEDYPLANNER] [robot %d] next goal: (%d,%d)\n",rbase->getNumber(),goal.x, goal.y); 00266 00267 } */ 00268 // visualizacion 00269 // if (showPlanner){ 00270 // cvCircle(im,cvPoint(poscell.x, omap->getHeight()-1-poscell.y),0,CV_RGB(255,0,0),-1); //posicion rojo 00271 // 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 00272 // for (unsigned int i = 0;i < path.size(); i++) 00273 // cvCircle(im,cvPoint(path[i].x, omap->getHeight()-1-path[i].y),0,CV_RGB(255,0,255),-1); // path magenta 00274 // cvShowImage(mywindowstr,im); 00275 // cvWaitKey(5); 00276 // cvReleaseImage(&im); 00277 // } 00278 00279 if (omap) delete omap; 00280 if (ppmap) delete ppmap; 00281 00282 // modificar los enables/goal de la parte reactiva 00283 if (completedPath) reac->disableAll(); // paramos el robot hasta planificar de nuevo 00284 else{ 00285 // if (DEBUG) printf("[GREEDYPLANNER] [robot %d] ************************************** Following Path\n",rbase->getNumber()); 00286 00287 reac->setGoal(goal); 00288 reac->enableGoToGoal(); 00289 reac->enableAvoidObstacles(); 00290 reac->disableAvoidOtherRobots(); 00291 reac->disableGoToFrontier(); 00292 reac->disableGoToUnexploredZones(); 00293 reac->disableGoToPrecisePoses(); 00294 } 00295 00296 // write logs 00297 if (logstr) fprintf(plannerlog,"%d, %d, %d, %d\n", nextStep, completedPath, poscell.x, poscell.y); 00298 } 00299 00300 // if (showPlanner) cvDestroyWindow(mywindowstr); 00301 00302 //closing logs 00303 if (logstr) fclose(plannerlog); 00304 00305 // if (DEBUG) printf("[GREEDYPLANNER] [robot %d] stoping robot...\n", rbase->getNumber()); 00306 reac->disableAll(); 00307 reac->stop(); 00308 closing.unlock(); 00309 explorationFinished.step(); 00310 // if (DEBUG) printf("[GREEDYPLANNER] \t\t\t\t\t\t\t [robot %d] robot planner stopped\n", rbase->getNumber()); 00311 } 00312
1.7.6.1
