|
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 #include "simulatedModel.h" 00002 #include "ConfigFile.h" 00003 #include "StringTokenizer.h" 00004 #include <iostream> 00005 #include "matFuns.h" 00006 #ifndef WIN32 00007 #include <sys/time.h> 00008 #include <pthread.h> 00009 #endif 00010 #include <QDir> 00011 00012 #ifdef WIN32 00013 #include <direct.h> // getcwd() gets current working dir 00014 #endif 00015 00016 #define MAXACQUIREDLANDMARKS 60 00017 00018 //TODO: Hacer el numero de robots dinámico a medida que llegan getids() 00019 00020 simulatedModel::simulatedModel(): 00021 nBots(0), 00022 maxNumRobots(0), 00023 step(0), 00024 rsData(0), 00025 lmData(0), 00026 odo(0), 00027 gt(0), 00028 lastodo(0), 00029 lastgt(0), 00030 rbase(0), 00031 robotsEnabled(0), 00032 velo(0), 00033 logstr(0), 00034 alfa1(0.0f), 00035 alfa2(0.0f), 00036 alfa3(0.0f), 00037 alfa4(0.0f), 00038 drifttrans(1.0f), 00039 imgwidth(0), 00040 imgheight(0), 00041 f(0.0f), 00042 I(0.0f), 00043 sigma2r(0.0f), 00044 sigma2c(0.0f), 00045 sigma2cp(0.0f), 00046 sigmar(0.0f), 00047 sigmac(0.0f), 00048 sigmacp(0.0f), 00049 betaMAX(0.0f), 00050 gammaMAX(0.0f), 00051 distMAX(0.0f), 00052 distMIN(0.0f), 00053 camx(0.0), 00054 camy(0.0), 00055 camz(0.0), 00056 rx(0.0), 00057 ry(0.0), 00058 rz(0.0), 00059 sample_time(0.0f), 00060 numSensors(0), 00061 devError(0.0f), 00062 realError(0.0f), 00063 gammamax(0.0f), 00064 aperture(0.0f), 00065 maxDist(0.0f), 00066 minDist(0.0f), 00067 //robots_lines(0), 00068 endth(true), 00069 idCounter(0), 00070 Scam(9,9), 00071 width(0), 00072 height(0), 00073 showsimulation(false), 00074 // groupInitialPoses (false), 00075 // maxInitialPoseDist (0), 00076 numfeat (0), 00077 mapLoaded(false), 00078 omap(0), 00079 omapfilename(new char[50]) 00080 { 00081 printf("[SIM] Default simulated model created\n"); 00082 } 00083 00084 simulatedModel::simulatedModel(int b, ConfigFile& config, float sampletime): 00085 nBots (b), 00086 maxNumRobots (config.read<int>("MAXNUMROBOTS")), 00087 step (0), 00088 rsData (new rangeSensorData[maxNumRobots]), 00089 lmData (new landmarksData[maxNumRobots]), 00090 odo (new pose[maxNumRobots]), 00091 gt (new pose[maxNumRobots]), 00092 lastodo (new pose[maxNumRobots]), 00093 lastgt (new pose[maxNumRobots]), 00094 rbase (new robotBase*[maxNumRobots]), 00095 robotsEnabled (new bool[maxNumRobots]), 00096 velo (new speed[maxNumRobots]), 00097 logstr (0), 00098 alfa1 (config.read<float>("alfa1")), 00099 alfa2 (config.read<float>("alfa2")), 00100 alfa3 (config.read<float>("alfa3")), 00101 alfa4 (config.read<float>("alfa4")), 00102 drifttrans (1.0), 00103 00104 imgwidth (config.read<int>("WIDTH")), 00105 imgheight (config.read<int>("HEIGHT")), 00106 00107 sigma2f (0.0), 00108 sigmaf (0.0), 00109 f (config.read<float>("f")), 00110 Realf (normrnd(f,sigmaf)), 00111 00112 sigma2I (0.0), 00113 sigmaI (0.0), 00114 I (config.read<float>("I")), 00115 RealI (normrnd(I,sigmaI)), 00116 00117 sigma2Cx (0.0), 00118 sigmaCx (0.0), 00119 Cx (imgwidth/2.0), 00120 RealCx (normrnd(Cx,sigmaCx)), 00121 00122 sigma2Cy (0.0), 00123 sigmaCy (0.0), 00124 Cy (imgheight/2.0), 00125 RealCy (normrnd(Cy,sigmaCy)), 00126 00127 sigma2Cxp (0.0), 00128 sigmaCxp (0.0), 00129 Cxp (imgwidth/2.0), 00130 RealCxp (normrnd(Cxp,sigmaCxp)), 00131 00132 sigmar (config.read<float>("sigmar")), 00133 sigmac (config.read<float>("sigmac")), 00134 sigmacp (0.0), 00135 sigmad (config.read<float>("sigmad")), 00136 sigma2r (pow(sigmar,2)), 00137 sigma2c (pow(sigmac,2)), 00138 sigma2cp (0.0), 00139 sigma2d (pow(sigmad,2)), 00140 00141 betaMAX (atan2(Cy,f)), 00142 gammaMAX (atan2(Cx,f)), 00143 00144 distMAX (config.read<float>("distMAX")), 00145 distMIN (config.read<float>("distMIN")), 00146 camx (config.read<float>("camx")), 00147 camy (config.read<float>("camy")), 00148 camz (config.read<float>("camz")), 00149 rx (0.0), 00150 ry (0.0), 00151 rz (0.0), 00152 00153 numSensors (config.read<int>("NUMPOINTS")), 00154 devError (config.read<float>("LASERSIGMA")), 00155 realError (devError), 00156 gammamax (config.read<float>("LASERMAXANGLE")*PI/180.0), 00157 aperture (0.0), 00158 maxDist (config.read<float>("LASERMAXDIST")), 00159 minDist (config.read<float>("LASERMINDIST")), 00160 sample_time (sampletime), 00161 //robots_lines (new std::vector<line>[nBots]), 00162 endth (true), 00163 idCounter (0), 00164 Scam (9,9), 00165 // width (config.read<int>("SIMULATORWIDTH")), 00166 // height (config.read<int>("SIMULATORHEIGHT")), 00167 width (800), 00168 height (600), 00169 // showsimulation (config.read<bool>("SHOWSIMULATION")), 00170 showsimulation (false), 00171 //groupInitialPoses (false), 00172 //maxInitialPoseDist (0), 00173 numfeat (0), 00174 mapLoaded (false), 00175 omap (0), 00176 omapfilename (new char[50]) 00177 { 00178 robots_lines.reserve(maxNumRobots); 00179 00180 string robotpos = config.read<string>("FOOTPRINT"); 00181 StringTokenizer st(&robotpos[0]); 00182 robot_footprint.clear(); 00183 for (int t = 0; t < st.countTokens(); t++){ 00184 float pt = atof(st.nextToken()); 00185 robot_footprint.push_back(pt); 00186 } 00187 00188 // Matriz de covarianza del sensor 00189 Scam.set(0,0,sigma2c); // S = [sigma2c 0 0 0 0 0 0 0 ] 00190 Scam.set(1,1,sigma2r); // [ 0 sigma2r 0 0 0 0 0 0 ] 00191 Scam.set(2,2,sigma2cp); // [ 0 0 sigma2cp 0 0 0 0 0 ] 00192 Scam.set(3,3,sigma2Cx); // [ 0 0 0 sigma2Cx 0 0 0 0 ] 00193 Scam.set(4,4,sigma2Cy); // [ 0 0 0 0 sigma2Cy 0 0 0 ] 00194 Scam.set(5,5,sigma2Cxp);// [ 0 0 0 0 0 sigma2Cxp 0 0 ] 00195 Scam.set(6,6,sigma2f); // [ 0 0 0 0 0 0 sigma2f 0 ] 00196 Scam.set(7,7,sigma2I); // [ 0 0 0 0 0 0 0 sigma2I ] 00197 Scam.set(8,8,sigma2d); 00198 00199 robots_lines.clear(); 00200 00201 for (int r=0; r<maxNumRobots; r++){ 00202 std::vector<line> v(robot_footprint.size()); 00203 robots_lines.push_back(v); 00204 } 00205 00206 reset(); 00207 00208 loadMapFile(config); 00209 00210 printf("[SIM] simulation model created\n"); 00211 } 00212 00213 void simulatedModel::reset(){ 00214 00215 idCounter=0; 00216 00217 for (int r=0; r<maxNumRobots; r++){ 00218 rsData[r].initialize(numSensors, devError, gammamax, aperture, maxDist, minDist); 00219 rbase[r] = 0; 00220 lmData[r].reserve(MAXACQUIREDLANDMARKS); 00221 robotsEnabled[r]=true; 00222 } 00223 } 00224 00225 void simulatedModel::setNumRobots(int n){ 00226 nBots = n; 00227 reset(); 00228 } 00229 00230 simulatedModel::~simulatedModel(){ 00231 stop(); 00232 if (rsData) delete[] rsData; 00233 if (lmData) delete[] lmData; 00234 if (odo) delete[] odo; 00235 if (gt) delete[] gt; 00236 if (lastodo) delete[] lastodo; 00237 if (lastgt) delete[] lastgt; 00238 if (velo) delete[] velo; 00239 if (logstr) delete[] logstr; 00240 if (rbase) delete[] rbase; 00241 if (robotsEnabled) delete[] robotsEnabled; 00242 if (omap) delete omap; 00243 if (omapfilename) delete[] omapfilename; 00244 00245 printf("[SIM] Simulation ended\n"); 00246 } 00247 00248 // initializer 00249 void simulatedModel::initialize(int b, ConfigFile& config, float sampletime){ 00250 00251 printf("initializing simulation model...\n"); 00252 nBots = b; 00253 maxNumRobots = config.read<int>("MAXNUMROBOTS"); 00254 step = 0; 00255 if (rsData) delete[] rsData; 00256 rsData = new rangeSensorData[maxNumRobots]; 00257 if (lmData) delete[] lmData; 00258 lmData = new landmarksData[maxNumRobots]; 00259 if (odo) delete[] odo; 00260 odo = new pose[maxNumRobots]; 00261 if (gt) delete[] gt; 00262 gt = new pose[maxNumRobots]; 00263 if (lastodo) delete[] lastodo; 00264 lastodo = new pose[maxNumRobots]; 00265 if (lastgt) delete[] lastgt; 00266 lastgt = new pose[maxNumRobots]; 00267 if (velo) delete[] velo; 00268 velo = new speed[maxNumRobots]; 00269 if (logstr) delete[] logstr; 00270 logstr = 0; 00271 if (rbase) delete[] rbase; 00272 rbase = new robotBase*[maxNumRobots]; 00273 if (robotsEnabled) delete[] robotsEnabled; 00274 robotsEnabled = new bool[maxNumRobots]; 00275 if (omap) delete omap; 00276 omap = 0; 00277 if (omapfilename) delete[] omapfilename; 00278 omapfilename = new char[50]; 00279 00280 alfa1 = config.read<float>( "alfa1" ); 00281 alfa2 = config.read<float>( "alfa2" ); 00282 alfa3 = config.read<float>( "alfa3" ); 00283 alfa4 = config.read<float>( "alfa4" ); 00284 drifttrans = 1.0; 00285 00286 imgwidth = config.read<int>("WIDTH"); 00287 imgheight = config.read<int>("HEIGHT"); 00288 00289 sigma2f = 0.0; 00290 sigmaf = 0.0; 00291 f = config.read<float>("f"); 00292 Realf = normrnd(f,sigmaf); 00293 00294 sigma2I = 0.0; 00295 sigmaI = 0.0; 00296 I = config.read<float>("I"); 00297 RealI = normrnd(I,sigmaI); 00298 00299 sigma2Cx = 0.0; 00300 sigmaCx = 0.0; 00301 Cx = imgwidth/2.0; 00302 RealCx = normrnd(Cx,sigmaCx); 00303 00304 sigma2Cy = 0.0; 00305 sigmaCy = 0.0; 00306 Cy = imgheight/2.0; 00307 RealCy = normrnd(Cy,sigmaCy); 00308 00309 sigma2Cxp = 0.0; 00310 sigmaCxp = 0.0; 00311 Cxp = imgwidth/2.0; 00312 RealCxp = normrnd(Cxp,sigmaCxp); 00313 00314 sigmar = config.read<float>("sigmar"); 00315 sigmac = config.read<float>("sigmac"); 00316 sigmacp = 0.0; 00317 sigmad = config.read<float>("sigmad"); 00318 sigma2r = pow(sigma2r,2); 00319 sigma2c = pow(sigma2c,2); 00320 sigma2cp = pow(sigma2cp,2); 00321 sigma2d = pow(sigma2d,2); 00322 00323 betaMAX = atan2(Cy,f); 00324 gammaMAX = atan2(Cx,f); 00325 distMAX = config.read<float>( "distMAX" ); 00326 distMIN = config.read<float>( "distMIN" ); 00327 00328 camx = config.read<float>( "camx" ); 00329 camy = config.read<float>( "camy" ); 00330 camz = config.read<float>( "camz" ); 00331 rx = 0.0; 00332 ry = 0.0; 00333 rz = 0.0; 00334 00335 // sonar 00336 numSensors = config.read<int>("NUMPOINTS"); 00337 devError = config.read<float>("LASERSIGMA"); 00338 realError = devError; 00339 gammamax = config.read<float>("LASERMAXANGLE")*PI/180.0; 00340 aperture = 0.0; 00341 maxDist = config.read<float>("LASERMAXDIST"); 00342 minDist = config.read<float>("LASERMINDIST"); 00343 00344 sample_time = sampletime; 00345 00346 //robots_lines = new std::vector<line>[nBots]; 00347 robots_lines.clear(); 00348 robots_lines.reserve(maxNumRobots); 00349 00350 endth =true; 00351 idCounter=0; 00352 00353 // width=config.read<int>("SIMULATORWIDTH"); 00354 // height=config.read<int>("SIMULATORHEIGHT"); 00355 // showsimulation = config.read<bool>("SHOWSIMULATION"); 00356 00357 width=800; 00358 height=600; 00359 showsimulation = false; 00360 00361 //groupInitialPoses=false; 00362 //maxInitialPoseDist=0; 00363 numfeat=0; 00364 mapLoaded=false; 00365 00366 //--------- 00367 00368 string robotpos = config.read<string>("FOOTPRINT"); 00369 StringTokenizer st(&robotpos[0]); 00370 robot_footprint.clear(); 00371 for (int t = 0; t < st.countTokens(); t++){ 00372 float pt = atof(st.nextToken()); 00373 robot_footprint.push_back(pt); 00374 } 00375 00376 for (int r=0; r<maxNumRobots; r++){ 00377 std::vector<line> v(robot_footprint.size()); 00378 robots_lines.push_back(v); 00379 } 00380 00381 reset(); 00382 00383 // Matriz de covarianza del sensor 00384 Scam.clear(); 00385 Scam.set(0,0,sigma2c); // S = [sigma2c 0 0 0 0 0 0 0 ] 00386 Scam.set(1,1,sigma2r); // [ 0 sigma2r 0 0 0 0 0 0 ] 00387 Scam.set(2,2,sigma2cp); // [ 0 0 sigma2cp 0 0 0 0 0 ] 00388 Scam.set(3,3,sigma2Cx); // [ 0 0 0 sigma2Cx 0 0 0 0 ] 00389 Scam.set(4,4,sigma2Cy); // [ 0 0 0 0 sigma2Cy 0 0 0 ] 00390 Scam.set(5,5,sigma2Cxp);// [ 0 0 0 0 0 sigma2Cxp 0 0 ] 00391 Scam.set(6,6,sigma2f); // [ 0 0 0 0 0 0 sigma2f 0 ] 00392 Scam.set(7,7,sigma2I); // [ 0 0 0 0 0 0 0 sigma2I ] 00393 Scam.set(8,8,sigma2d); 00394 00395 loadMapFile(config); 00396 00397 printf("simulated model initialized\n"); 00398 00399 } 00400 00401 void simulatedModel::setLogName(const char* str){ 00402 if (logstr) delete[] logstr; 00403 logstr = new char[strlen(str)+1]; 00404 strcpy(logstr,str); 00405 } 00406 00407 void simulatedModel::update_footprint(int r){ 00408 robots_lines[r].clear(); 00409 int total_lines2 = robot_footprint.size(); 00410 for (int l = 0; l < robot_footprint.size(); l+=2){ 00411 float x1 = robot_footprint[l]; 00412 float y1 = robot_footprint[l+1]; 00413 float x2 = robot_footprint[(l+2)%total_lines2]; 00414 float y2 = robot_footprint[(l+3)%total_lines2]; 00415 line lin( gt[r].x+x1*cos(gt[r].th)-y1*sin(gt[r].th), 00416 gt[r].y+x1*sin(gt[r].th)+y1*cos(gt[r].th), 00417 gt[r].x+x2*cos(gt[r].th)-y2*sin(gt[r].th), 00418 gt[r].y+x2*sin(gt[r].th)+y2*cos(gt[r].th)); 00419 robots_lines[r].push_back(lin); 00420 } 00421 } 00422 00423 int simulatedModel::setup(){ 00424 prio = 50; 00425 return 0; 00426 } 00427 00428 void simulatedModel::onStop(){ 00429 //printf("[SIM] Simulator on stop...\n"); 00430 if(!endth){ 00431 endth = true; 00432 closing.lock(); 00433 closing.unlock(); 00434 } 00435 } 00436 00437 void simulatedModel::execute(){ 00438 printf("[SIM] \t\t\t\t\t\t\t Simulation Running\n"); 00439 closing.lock(); 00440 endth = false; 00441 00442 speed speedconst; 00443 speedconst.v=0.5f; 00444 speedconst.w=0.0f; 00445 00446 for (int r = 0; r< nBots; r++){ 00447 rbase[r]->beginProduction(); 00448 odo[r] = gt[r]; 00449 // printf("[SIM] generating initial state for robot %d...\n",r); 00450 simRangeAdquisition(gt[r], rsData[r],r); // leemos sonar 00451 lmData[r].clear(); 00452 simCamAdquisition(gt[r], lmData[r],r); // leemos landmarks 00453 lastgt[r] = gt[r]; 00454 lastodo[r] = odo[r]; 00455 // printf("[SIM] produced initial state for robot %d\n",r); 00456 rbase[r]->endProduction(); 00457 } 00458 00459 // printf("Parameters>>>>>>\n"); 00460 // printf("f = %f (%f)\n",Realf, f); 00461 // printf("I = %f (%f)\n",RealI, I); 00462 // printf("Cx = %f (%f)\n",RealCx, Cx); 00463 // printf("Cy = %f (%f)\n",RealCy, Cy); 00464 // printf("Cxp = %f (%f)\n",RealCxp, Cxp); 00465 // printf(">>>>>>>>>>>>>>>>>>\n"); 00466 00467 // open the log file 00468 FILE* vworldlog=0; 00469 if (logstr){ 00470 char myfilestr[100]; 00471 sprintf(myfilestr,"%s.m",logstr); 00472 vworldlog = fopen(myfilestr,"w"); 00473 fprintf(vworldlog,"ground_truth = [",logstr); 00474 } 00475 00476 char* wname= "Simulator"; 00477 IplImage* background = 0; 00478 if (showsimulation){ 00479 background = getMapImage(); 00480 cvNamedWindow(wname,0); 00481 } 00482 int capNum=0; 00483 char filestr[100]; 00484 00485 elapsedTime = 0.0f; 00486 int step=1; 00487 do{ 00488 //printf("[SIM] sleeping\n"); 00489 sleepms(100); 00490 for (int r = 0; r< nBots; r++){ 00491 if (robotsEnabled[r]){ 00492 //printf("[SIM] waiting to produce step %d for robot %d...\n", step,r); 00493 rbase[r]->beginProduction(); 00494 //printf("[SIM] generating step %d for robot %d...\n", step,r); 00495 if (robotsEnabled[r]){ 00496 if(step>2) robotMotion(gt[r], velo[r], sample_time); // move robots 00497 update_footprint(r); 00498 //gt[r].print("\t\t\t\t\t\t\t\t\tground truth="); 00499 //robotMotion(gt[r], speedconst, sample_time); // move robots 00500 motionSample(lastgt[r], gt[r], lastodo[r], odo[r]); // leemos odometria 00501 //odo[r].print("\t\t\t\t\t\t\t\t\todo="); 00502 simRangeAdquisition(gt[r], rsData[r],r); // leemos sonar 00503 lmData[r].clear(); 00504 simCamAdquisition(gt[r], lmData[r],r); // leemos landmarks 00505 lastgt[r] = gt[r]; 00506 lastodo[r] = odo[r]; 00507 } 00508 //printf("[SIM] produced data for step %d and robot %d\n",step,r); 00509 } 00510 rbase[r]->endProduction(); 00511 } 00512 step++; 00513 // write logs 00514 if (logstr){ 00515 fprintf(vworldlog,"%f",elapsedTime); 00516 for (int r=0; r<nBots;r++){ 00517 fprintf(vworldlog,", %f, %f, %f",gt[r].x, gt[r].y, gt[r].th); 00518 } 00519 fprintf(vworldlog,"\n"); 00520 } 00521 elapsedTime+=sample_time; 00522 redrawing(); 00523 // if (showsimulation){ 00524 // IplImage* img = cvCloneImage(background); 00525 // drawRobots(img); 00526 // cvShowImage(wname,img); 00527 // int key = cvWaitKey(2); 00528 // if (key=='c'){ 00529 // sprintf(filestr,"simcap%d.png",capNum); 00530 // cvSaveImage(filestr,img); 00531 // capNum++; 00532 // } 00533 // cvReleaseImage(&img); 00534 // } 00535 } while (!endth); 00536 00537 if (showsimulation){ 00538 cvDestroyWindow(wname); 00539 cvReleaseImage(&background); 00540 } 00541 00542 if (logstr){ 00543 fprintf(vworldlog,"];"); 00544 fclose(vworldlog); 00545 // save img 00546 } 00547 00548 //printf("[SIM] closing unlock\n"); 00549 closing.unlock(); 00550 } 00551 00552 // TODO: Include error checking, it is likely to crash with an erroneous map file 00553 void simulatedModel::loadMapFile(const ConfigFile& config){ 00554 printf("[SIM] loading map...\n"); 00555 //ConfigFile config(mapfile); 00556 00557 // initial poses 00558 char entrada[20]; 00559 00560 // walls / obstacles 00561 int numwalls = config.read<int>("NUMWALLS"); 00562 mapWall.clear(); 00563 mapWall.reserve(numwalls); 00564 for (int i = 0; i< numwalls; i++){ 00565 sprintf(entrada,"WALL%i",i+1); 00566 string wallstr = config.read<string>(entrada); 00567 StringTokenizer st(&(wallstr[0])); 00568 float x1 = atof(st.nextToken()); 00569 float y1 = atof(st.nextToken()); 00570 float x2 = atof(st.nextToken()); 00571 float y2 = atof(st.nextToken()); 00572 line wall( x1, y1, x2, y2 ); 00573 mapWall.push_back(wall); 00574 } 00575 00576 // features 00577 numfeat = config.read<int>("NUMFEATURES"); 00578 desclength = config.read<int>("DESCRIPTORLENGTH"); 00579 float* desc = new float[desclength]; 00580 landmarks.clear(); 00581 landmarks.reserve(numfeat); 00582 for (int i = 0; i< numfeat; i++){ 00583 sprintf(entrada,"FEAT%i",i+1); 00584 string featstr = config.read<string>(entrada); 00585 StringTokenizer st(&(featstr[0])); 00586 float x = atof(st.nextToken()); 00587 float y = atof(st.nextToken()); 00588 float z = atof(st.nextToken()); 00589 for (int j=0; j < desclength; j++){ 00590 // desc[j] = atof(st.nextToken()); 00591 desc[j] = i; 00592 } 00593 landmarks.push_back(feature(pos3d(x,y,z),desc,desclength)); 00594 } 00595 delete[] desc; 00596 00597 //initial poses 00598 for (int r = 0; r< maxNumRobots; r++){ 00599 sprintf(entrada,"R%i",r+1); 00600 string robotpos = config.read<string>(entrada); 00601 StringTokenizer st(&robotpos[0]); 00602 00603 float x = atof(st.nextToken()); 00604 float y = atof(st.nextToken()); 00605 float th = atof(st.nextToken()); 00606 pose pos(x,y,th); 00607 odo[r] = pos; 00608 lastodo[r] = pos; 00609 gt[r] = pos; 00610 lastgt[r] = pos; 00611 update_footprint(r); 00612 } 00613 00614 // if (config.read<bool>("RANDOM")){ 00615 if (omap) delete omap; 00616 omap = gridMapFactory::Instance().CreateObject(config.read<int>("GRIDMAPTYPE"),0.0,0.0,0.0,0.0,0.0); 00617 strcpy(omapfilename, ((QDir::homePath()+="/.mrxt/maps/")+=config.read<string>("GRIDMAP").c_str()).toStdString().c_str()); 00618 omap->loadMapFromFile(omapfilename); 00619 // } 00620 00621 // cvNamedWindow("OMAP"); 00622 // omap->showMap("OMAP"); 00623 // cvWaitKey(5000); 00624 00625 // groupInitialPoses = true; //config.read<bool>("GROUPED"); 00626 // maxInitialPoseDist = 6.0/omap->getResolution(); // config.read<float>("MAXGROUPDIST")/omap->getResolution(); 00627 // minInitialPoseDist = 0.85; //config.read<float>("MINGROUPDIST"); 00628 00629 printf("[SIM] map loaded\n"); 00630 00631 mapLoaded = true; 00632 } 00633 00634 void simulatedModel::randomPoses(bool groupInitialPoses, double max, double min){ 00635 binMap visibleArea; 00636 00637 double maxInitialPoseDist = max/omap->getResolution(); // config.read<float>("MAXGROUPDIST")/omap->getResolution(); 00638 double minInitialPoseDist = min; //config.read<float>("MINGROUPDIST"); 00639 00640 // if (config.read<bool>("RANDOM")){ 00641 if(mapLoaded){ 00642 // printf("aqui\n"); 00643 binMap accessible; 00644 // printf("aqui\n"); 00645 point p = omap->toCell(odo[0].x, odo[0].y); 00646 // printf("aqui\n"); 00647 omap->countAccessible(&p, 1, accessible); 00648 // printf("pose robot 1: (%f, %f), in cells (%d, %d)\n", odo[0].x, odo[0].y, p.x, p.y); 00649 point basePoint; 00650 std::vector<point> positives; 00651 bool vis; 00652 int maxIter = 10000; 00653 int countIter=0; 00654 bool error=false; 00655 00656 do{ 00657 for (int r = 0; r< maxNumRobots; r++){ 00658 int x,y; 00659 bool testRange=true; 00660 00661 countIter=0; 00662 error = false; 00663 pointf pos; 00664 do{ 00665 countIter++; 00666 if (countIter>maxIter){ 00667 //printf("Solution not found, trying again\n"); 00668 error=true; 00669 break; 00670 } 00671 vis=false; 00672 if (!groupInitialPoses){ 00673 x = rand() % omap->getWidth(); 00674 y = rand() % omap->getHeight(); 00675 vis = true; 00676 } 00677 else if (r==0){ 00678 x = rand() % omap->getWidth(); 00679 y = rand() % omap->getHeight(); 00680 basePoint.x = x; 00681 basePoint.y = y; 00682 visibleArea.clear(); 00683 positives.clear(); 00684 omap->esz(x,y,visibleArea, 0,maxInitialPoseDist); 00685 if (visibleArea.getPositives(positives)<30) vis = false; 00686 else vis = true; 00687 } 00688 else{ 00689 int idx = rand() % positives.size(); 00690 x = positives[idx].x; 00691 y = positives[idx].y; 00692 vis = visibleArea.get(x,y); 00693 } 00694 00695 pos = omap->toCoords(x,y); 00696 testRange=true; 00697 for(int rr = 0; rr < r; rr++){ 00698 if( sqrt( pow(pos.x-gt[rr].x ,2) + pow(pos.y-gt[rr].y,2) ) < minInitialPoseDist ) testRange = false; 00699 } 00700 } while (!accessible.get(x,y) || !omap->isfree(x,y,4) || !vis || !testRange); 00701 00702 if (error) break; 00703 landmarksData lma; 00704 lma.reserve(numfeat); 00705 pose newpos(pos.x,pos.y,0); 00706 do{ 00707 lma.clear(); 00708 newpos.th = (rand() % 628)/100.0f-3.14f; 00709 simCamAdquisition(newpos, lma, r); 00710 // printf("th =%f, marks = %d\n",newpos.th, lma.getNLandmarks()); 00711 } while(lma.getNLandmarks() < 4); 00712 00713 //printf("[SIM] new robot position for robot %d: cells (%d,%d), coords (%f,%f)\n", r, x, y, pos.x, pos.y); 00714 odo[r] = newpos; 00715 lastodo[r] = newpos; 00716 gt[r] = newpos; 00717 lastgt[r] = newpos; 00718 update_footprint(r); 00719 } 00720 00721 } while(error); 00722 } 00723 } 00724 00725 00726 void simulatedModel::redrawing(){ 00727 //QPixmap* pixmap = getPixmap(); 00728 //emit redraw(*pixmap); 00729 //delete pixmap; 00730 00731 //emit () 00732 00733 //printf("emit poses\n"); 00734 rposes poses; 00735 for(int r = 0; r < nBots; r++){ 00736 poses.push_back(gt[r]); 00737 } 00738 emit changedPositions(poses); 00739 } 00740 00741 QPixmap* simulatedModel::getPixmap(){ 00742 IplImage* img = getMapImage(); 00743 drawRobots(img); 00744 cvSaveImage("imgtmp.png",img); 00745 cvReleaseImage(&img); 00746 QPixmap* pix = new QPixmap("imgtmp.png"); 00747 return pix; 00748 } 00749 00750 rposes simulatedModel::getPoses() const{ 00751 rposes poses; 00752 for(int r = 0; r < nBots; r++){ 00753 poses.push_back(gt[r]); 00754 } 00755 return poses; 00756 } 00757 00758 void simulatedModel::drawRobots(IplImage*& img){ 00759 CvScalar redcolor = CV_RGB(255,0,0); 00760 CvScalar bluecolor = CV_RGB(0,0,255); 00761 CvScalar blackcolor = CV_RGB(0,0,0); 00762 CvFont font; 00763 cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1, 1); 00764 char numstr[3]; 00765 00766 std::vector<line>::iterator lineit; 00767 for (int r = 0; r < nBots; r++){ 00768 for ( lineit = robots_lines[r].begin(); lineit < robots_lines[r].end(); lineit++ ){ 00769 CvPoint pt1 = cvPoint((int)((lineit->x1-minx)*scalex), height-(int)((lineit->y1-miny)*scaley) ); 00770 CvPoint pt2 = cvPoint((int)((lineit->x2-minx)*scalex), height-(int)((lineit->y2-miny)*scaley) ); 00771 //printf("(%d, %d) - (%d, %d)\n", pt1.x, pt1.y, pt2.x, pt2.y); 00772 //if (rbase[r]){ 00773 // if (rbase[r]->isCaptured()) 00774 // cvLine(img, pt1, pt2, bluecolor); 00775 // else 00776 cvLine(img, pt1, pt2, redcolor); 00777 //} 00778 } 00779 sprintf(numstr,"%d",r); 00780 CvPoint ptA = cvPoint((int)((gt[r].x-minx)*scalex), height-(int)((gt[r].y-miny)*scaley) ); 00781 CvPoint ptB = cvPoint(ptA.x+10*cos(gt[r].th),ptA.y-10*sin(gt[r].th)); 00782 CvPoint ptT = cvPoint(ptA.x+10,ptA.y+10); 00783 cvPutText(img, numstr, ptT, &font, blackcolor); 00784 //if (rbase[r]){ 00785 //if (rbase[r]->isCaptured()) 00786 // cvLine(img,ptA,ptB,bluecolor); 00787 //else 00788 cvLine(img,ptA,ptB,redcolor); 00789 //} 00790 } 00791 } 00792 00793 IplImage* simulatedModel::getMapImage() { 00794 00795 CvScalar blackcolor = CV_RGB(0,0,0); 00796 CvScalar whitecolor = CV_RGB(255,255,255); 00797 00798 IplImage* img = cvCreateImage(cvSize(width,height), IPL_DEPTH_8U, 3); 00799 cvSet(img,whitecolor); 00800 00801 std::vector<line>::iterator lineit; 00802 00803 // buscamos maximo y minimo 00804 // printf("\t\t\t\tnum walls = %d\n",mapWall.size()); 00805 maxx=FLT_MIN; maxy=FLT_MIN; minx=FLT_MAX; miny=FLT_MAX; 00806 for ( lineit = mapWall.begin(); lineit < mapWall.end(); lineit++ ){ 00807 if (maxx < lineit->x1) maxx= lineit->x1; 00808 if (maxx < lineit->x2) maxx= lineit->x2; 00809 if (maxy < lineit->y1) maxy= lineit->y1; 00810 if (maxy < lineit->y2) maxy= lineit->y2; 00811 if (minx > lineit->x1) minx= lineit->x1; 00812 if (minx > lineit->x2) minx= lineit->x2; 00813 if (miny > lineit->y1) miny= lineit->y1; 00814 if (miny > lineit->y2) miny= lineit->y2; 00815 } 00816 scalex = width/(maxx-minx); 00817 scaley = height/(maxy-miny); 00818 00819 // printf("xrange: [%f-%f], yrange: [%f-%f]\n", minx, maxx, miny, maxy); 00820 // printf("scalex: %f, scaley: %f", scalex, scaley); 00821 00822 for ( lineit = mapWall.begin(); lineit < mapWall.end(); lineit++ ){ 00823 CvPoint pt1 = cvPoint((int)((lineit->x1-minx)*scalex), height-(int)((lineit->y1-miny)*scaley) ); 00824 CvPoint pt2 = cvPoint((int)((lineit->x2-minx)*scalex), height-(int)((lineit->y2-miny)*scaley) ); 00825 // printf("[SIM] (%d, %d) - (%d, %d)\n", pt1.x, pt1.y, pt2.x, pt2.y); 00826 cvLine(img, pt1, pt2, blackcolor); 00827 } 00828 00829 return img; 00830 00831 } 00832 00833 // Hace avanzar al robot durante un tiempo sample_time a velocidad vel 00834 void simulatedModel::robotMotion(pose &pos, const speed& vel, float st){ 00835 float vt = st*vel.v; 00836 float wt = st*vel.w; 00837 float meanang = pos.th + wt/2.0f; 00838 pos.x += vt*cos(meanang); 00839 pos.y += vt*sin(meanang); 00840 pos.th+= wt; 00841 if (pos.th>M_PI) pos.th -= PIx2; 00842 else if (pos.th <= -M_PI) pos.th += PIx2; 00843 } 00844 00845 // Modelo de odometria 00846 void simulatedModel::motionSample(const pose& lastPose, const pose& newPose, const pose& lastodo, pose& newodo){ 00847 00848 float diffy = newPose.y-lastPose.y; 00849 float diffx = newPose.x-lastPose.x; 00850 float diffth = newPose.th-lastPose.th; 00851 if (!(diffx == 0 && diffy == 0 && diffth ==0)){ 00852 float deltarot1; 00853 //if (!(fabs(diffx) < 0.000001 && fabs(diffy) < 0.000001)) 00854 deltarot1 = atan2(diffy, diffx) - lastPose.th; // cuanto ha rotado 00855 //else 00856 // deltarot1 = 0; 00857 float deltatrans = drifttrans*sqrt((diffx*diffx + diffy*diffy)); // distancia avanzada 00858 float deltarot2 = diffth - deltarot1; // incremento de orientacion aparte de la rotacion 00859 00860 // float dtheta = fabs(atan2(sin(diffth),cos(diffth))); 00861 00862 // float aux = (alfa1*dtheta + alfa2*deltatrans); 00863 // float deltarot1g = (float)(normrnd(deltarot1, aux)); 00864 // float deltatransg = (float)(normrnd(deltatrans, alfa3*deltatrans + alfa4*dtheta)); 00865 // float deltarot2g = (float)(normrnd(deltarot2, aux)); 00866 00867 if (deltarot1>M_PI) deltarot1 -= PIx2; 00868 else if (deltarot1 <= -M_PI) deltarot1 += PIx2; 00869 if (deltarot2>M_PI) deltarot2 -= PIx2; 00870 else if (deltarot2 <= -M_PI) deltarot2 += PIx2; 00871 00872 float rot1n = alfa1*fabs(deltarot1) + alfa2*deltatrans; 00873 float trann = alfa3*deltatrans + alfa4*(fabs(deltarot1+deltarot2)); 00874 float rot2n = alfa1*fabs(deltarot2) + alfa2*deltatrans; 00875 float deltarot1g = (float)(normrnd(deltarot1, rot1n)); 00876 float deltatransg = (float)(normrnd(deltatrans, trann)); 00877 float deltarot2g = (float)(normrnd(deltarot2, rot2n)); 00878 00879 float aux2 = lastodo.th+deltarot1g; 00880 00881 newodo.x = lastodo.x + deltatransg * cos(aux2); 00882 newodo.y = lastodo.y + deltatransg * sin(aux2); 00883 newodo.th = lastodo.th + deltarot1g + deltarot2g; 00884 00885 if (newodo.th>M_PI) newodo.th -= PIx2; 00886 else if (newodo.th <= -M_PI) newodo.th += PIx2; 00887 } 00888 else{ 00889 newodo.x = lastodo.x; 00890 newodo.y = lastodo.y; 00891 newodo.th = lastodo.th; 00892 } 00893 00894 } 00895 00896 // TODO: incluir una probabilidad de que una landmark no se detecte aun estando en el rango de vision 00897 void simulatedModel::simCamAdquisition(const pose& robotpos, landmarksData& lmData, int nr){ 00898 00899 std::vector<feature>::iterator markit; 00900 pose3d campos = camPose(robotpos); 00901 //printf("robot pose x=%f, y=%f, th=%f\n", robotpos.x, robotpos.y, robotpos.th); 00902 for ( markit = landmarks.begin(); markit != landmarks.end(); markit++ ){ //comprobar la distancia a todas las landmarks del mapa 00903 pos3d v = base2cam(global2base(markit->pos, robotpos)); // posicion relativa de la marca a la camara 00904 float r = sqrt(v.getX()*v.getX()+v.getY()*v.getY()); // distancia de la camara a la marca 00905 00906 double gamma = atan2(v.getY(), v.getX()); 00907 double beta = atan2(v.getZ(), v.getX()); 00908 if (gamma >= PI) gamma-=PIx2; 00909 if (gamma < -PI) gamma+=PIx2; 00910 if (beta >= PI) beta-=PIx2; 00911 if (beta < -PI) beta+=PIx2; 00912 00913 //Si esta dentro del FOV del robot comprobamos su visibilidad dentro del mapa 00914 if ((r < distMAX) && (r > distMIN) && (fabs(gamma) < gammaMAX) && (fabs(beta) < betaMAX)){ 00915 00916 bool visibility = testVisibility(campos.pos, markit->pos, nr); 00917 00918 //printf("vis= %d || r = %f [%f - %f], gamma = %f (%f), beta = %f (%f)\n", visibility, r,distMAX,distMIN, gamma, gammaMAX, beta, betaMAX); 00919 00920 // si cae dentro del radio de accion del robot y es visible 00921 // Si cae dentro del FOV entonces recogemos la medida, anyadimos en simulacion un error a la medida segun 00922 // una ley de propagacion del error... ver lineas de investigacion 00923 if (visibility){ 00924 00925 // proyectamos el punto real en la camara 00926 float aux1 = Realf*RealI; 00927 double c = -Realf*v.getY() / v.getX() + RealCx; 00928 double r = Realf*v.getZ() / v.getX() + RealCy; 00929 //double cp = -Realf*(RealI+v.getY())/ v.getX() + RealCxp; 00930 double d = aux1/v.getX() ;//(c - RealCx) - (cp - RealCxp); 00931 double d2 = d*d; 00932 00933 if(c<0 || r<0 || c>2*Cx || r>2*Cy ) continue; 00934 //printf("sigmas: %f, %f, %f\n",sigmac, sigmar, sigmad); 00935 00936 // Añadimos ruido a (Xc, Yc, Zc) (Ruido gaussiano en r,c,d y propagamos) 00937 double cprev = c; 00938 do{ 00939 c = normrnd(cprev,sigmac); 00940 }while(fabs(c-cprev)>3*sigmac); 00941 00942 double rprev = r; 00943 do{ 00944 r = normrnd(rprev,sigmar); 00945 }while(fabs(r-rprev)>3*sigmar); 00946 00947 // cp = normrnd(cp,sigmacp); 00948 00949 //d = (c - Cx) - (cp - Cxp); 00950 double dprev = d; 00951 do{ 00952 d = normrnd(dprev,sigmad); 00953 }while(fabs(d-dprev)>3*sigmad); 00954 d2 = d*d; 00955 double d2prev = dprev*dprev; 00956 00957 // v.print("VW MARK = "); 00958 00959 // Obtenemos la pose a partir de las medidas simuladas 00960 aux1 = f*I; 00961 float aux3 = I/d; 00962 v.getX() = aux3*f; 00963 v.getY() = aux3*(Cx-c); 00964 v.getZ() = aux3*(r-Cy); 00965 00966 /* 00967 // Jacobiano 00968 matrix J(3,8); 00969 J.set(0,0, -aux1/d2); 00970 J.set(0,2, aux1/d2); 00971 J.set(0,3, aux1/d2); 00972 J.set(0,5, -aux1/d2); 00973 J.set(0,6, I/d); 00974 J.set(0,7, f/d); 00975 00976 J.set(1,0, -I/d - I*(Cx-c)/d2); 00977 J.set(1,2, I*(Cx-c)/d2); 00978 J.set(1,3, I/d + I*(Cx-c)/d2); 00979 J.set(1,5, -I*(Cx-c)/d2); 00980 J.set(1,7, (Cx-c)/d); 00981 00982 J.set(2,0, -I*(r-Cy)/d2); 00983 J.set(2,1, I/d); 00984 J.set(2,2, I*(r-Cy)/d2); 00985 J.set(2,3, I*(r-Cy)/d2); 00986 J.set(2,4, -I/d); 00987 J.set(2,5, -I*(r-Cy)/d2); 00988 J.set(2,7, (r-Cy)/d); 00989 */ 00990 // printf("Cx=%f, Cy=%f, Cxp=%f, f=%f, I=%f\n",Cx,Cy,Cxp,f,I); 00991 // printf("c=%f, r=%f, cp=%f, d=%f d2=%f\n",c,r,cp,d,d2); 00992 00993 matrix J(3,9); 00994 J.set(0,8, -aux1/d2); 00995 J.set(1,0, -I/d); 00996 J.set(1,8, I*(c-Cx)/d2); 00997 J.set(2,1, I/d); 00998 J.set(2,8, -I*(r-Cy)/d2); 00999 01000 // Covarianza de la medida 01001 // v.print("V="); 01002 // printf("c= %f, r= %f, d=%f\n",c,r,d); 01003 // printf("sigmac = %f, sigmar = %f, sigmad = %f\n", sigmac, sigmar, sigmad); 01004 // printf("sigma2c = %e, sigma2r = %e, sigma2d = %e\n", sigma2c, sigma2r, sigma2d); 01005 // J.print("J="); 01006 // Scam.print("Scam="); 01007 matrix cov = J*(Scam*J.transpose()); 01008 // cov.print("cov="); 01009 // cov*=1.5; 01010 01011 //printf("c=%f, r=%f, cp=%f, d=%f\n",c,r,cp,d); 01012 //v.print("VW MARK CON RUIDO = "); 01013 // cov.print("COV="); 01014 01015 // añadimos una marca a lmdata 01016 landmark lmark(v,cov,markit->desc,desclength,0); 01017 //printf("voy a anyadir\n"); 01018 lmData.addLandmark(lmark); 01019 //printf("anyadida\n"); 01020 //if( lmData.getNLandmarks() > 10) break; 01021 } // visible 01022 } // in the FOV 01023 }// for each landmark 01024 //printf("[VIRTUALWORLD] observed features: %d\n", lmData.getNLandmarks()); 01025 } 01026 01027 // TODO: use the sensor pose information 01028 void simulatedModel::simRangeAdquisition(const pose& realPose, rangeSensorData& sonarData, int r){ 01029 pose dest; 01030 for (int i = 0; i < sonarData.getNumSensors(); i++ ) { 01031 float ang = realPose.th+ sonarData.getSensorPose(i).th + aperture/2; 01032 dest.x = realPose.x + sonarData.getMaxDist()*cos(ang); 01033 dest.y = realPose.y + sonarData.getMaxDist()*sin(ang); 01034 float dist1 = visibilityDistance(realPose, dest,r); 01035 01036 ang = realPose.th+ sonarData.getSensorPose(i).th - aperture/2; 01037 dest.x = realPose.x + sonarData.getMaxDist()*cos(ang); 01038 dest.y = realPose.y + sonarData.getMaxDist()*sin(ang); 01039 float dist2 = visibilityDistance(realPose, dest,r); 01040 01041 ang = realPose.th+ sonarData.getSensorPose(i).th; 01042 dest.x = realPose.x + sonarData.getMaxDist()*cos(ang); 01043 dest.y = realPose.y + sonarData.getMaxDist()*sin(ang); 01044 float dist3 = visibilityDistance(realPose, dest,r); 01045 01046 float dist = (dist1<dist2)? ((dist1<dist3)? dist1: dist3) : ((dist2<dist3)? dist2: dist3) ; 01047 sonarData.setSensorValue(i,normrnd(dist, pow(realError*dist/3.0f,2))); 01048 } 01049 } 01050 01051 bool simulatedModel::testVisibility(const pos3d& pos, const pos3d& mark, int nr){ 01052 01053 int suma = 0; 01054 pointf crosspoint; 01055 line ray(pos.getX(),pos.getY(),mark.getX(),mark.getY()); 01056 01057 //Comprobamos las veces que corta con alguna de las rectas del mapa 01058 std::vector<line>::iterator lineit; 01059 for ( lineit = mapWall.begin(); lineit < mapWall.end(); lineit++ ){ 01060 int inter = intersect(ray, *lineit, crosspoint); 01061 if (!(fabs(crosspoint.x-mark.getX())<0.05 && fabs(crosspoint.y -mark.getY())<0.05 )) suma+=inter; 01062 } 01063 // for (int r = 0; r < nBots; r++){ 01064 // if (r == nr) continue; 01065 // for ( lineit = robots_lines[r].begin(); lineit < robots_lines[r].end(); lineit++ ){ 01066 // int inter = intersect(ray, *lineit, crosspoint); 01067 // if (!(fabs(crosspoint.x-mark.getX())<0.05 && fabs(crosspoint.y -mark.getY())<0.05 )) suma+=inter; 01068 // } 01069 // } 01070 // si unicamente hemos detectado un corte, entonces es visible. Si corta 2 o 01071 // mas veces, entonces el punto atraviesa alguna pared 01072 // se supone q las marcas estan sobre las paredes por lo tanto hay un corte 01073 if (suma < 1 ) 01074 return true; 01075 else 01076 return false; 01077 } 01078 01079 float simulatedModel::visibilityDistance(const pose& ori, const pose& dest, int nr){ 01080 pointf crosspoint; 01081 01082 01083 01084 line ray(ori.x,ori.y,dest.x,dest.y); 01085 float diffx = ori.x-dest.x; 01086 float diffy = ori.y-dest.y; 01087 float dist = diffx*diffx+diffy*diffy; 01088 01089 //Comprobamos las veces que corta con alguna de las rectas del mapa 01090 std::vector<line>::iterator lineit; 01091 01092 for ( lineit = mapWall.begin(); lineit < mapWall.end(); lineit++ ){ 01093 if (intersect(ray, *lineit, crosspoint)){ 01094 diffx = ori.x-crosspoint.x; 01095 diffy = ori.y-crosspoint.y; 01096 float aux = diffx*diffx+diffy*diffy; 01097 if (aux < dist) dist = aux; 01098 } 01099 } 01100 // for (int r = 0; r < nBots; r++){ 01101 // if (r == nr) continue; 01102 // for ( lineit = robots_lines[r].begin(); lineit < robots_lines[r].end(); lineit++ ){ 01103 // if (intersect(ray, *lineit, crosspoint)){ 01104 // diffx = ori.x-crosspoint.x; 01105 // diffy = ori.y-crosspoint.y; 01106 // float aux = diffx*diffx+diffy*diffy; 01107 // if (aux < dist) dist = aux; 01108 // } 01109 // } 01110 // } 01111 return (sqrt(dist)); 01112 } 01113 01114 // TODO: consider rotation angles rx, ry, rz 01115 pose3d simulatedModel::camPose(const pose& robotpose){ 01116 float cth = cos(robotpose.th); 01117 float sth = sin(robotpose.th); 01118 return pose3d( robotpose.x + camx*cth+camy*sth, robotpose.y + camx*sth+camy*cth, camz, robotpose.th); 01119 } 01120 01121 // TODO: use calibration rx, ry, rz 01122 // translates a position in the camera frame of reference to the robot's frame of reference 01123 pos3d simulatedModel::cam2base(const pos3d& p){ 01124 return pos3d(p.getX() + camx, 01125 p.getY() + camy, 01126 p.getZ() + camz); 01127 } 01128 01129 // translates a position in the robot's frame of reference to the camera frame of reference 01130 pos3d simulatedModel::base2cam(const pos3d& p){ 01131 return pos3d(p.getX() - camx, 01132 p.getY() - camy, 01133 p.getZ() - camz); 01134 } 01135 01136 robotBase* simulatedModel::createNewPlatform(){ 01137 robotBase* res; 01138 newIdsMutex.lock(); 01139 if (idCounter<nBots){ 01140 rbase[idCounter] = new robotBase(idCounter); 01141 rbase[idCounter]->setWorldModel(this); 01142 res = rbase[idCounter]; 01143 idCounter++; 01144 } 01145 else res= 0; 01146 newIdsMutex.unlock(); 01147 //printf("the rbase created is at pointer %d",res); 01148 return res; 01149 } 01150 01151 void simulatedModel::disablePlatform(const robotBase& r){ 01152 int id = r.getNumber(); 01153 //printf("[SIM] Disabling robot...%d\n",id); 01154 if (robotsEnabled[id]){ 01155 rbase[id]->beginConsumition(); 01156 robotsEnabled[id]=false; 01157 rbase[id]->endConsumition(); 01158 } 01159 //printf("[SIM] robot %d disabled\n",id); 01160 } 01161 01162 int simulatedModel::fire(int robot){ 01163 int res = 0; 01164 pointf crosspoint; 01165 for (int r = 0; r < nBots; r++){ 01166 if (r == robot) continue; 01167 if (!rbase[r]) continue; 01168 01169 if (rbase[r]->isCaptured()) continue; 01170 01171 line ray(gt[robot].x,gt[robot].y,gt[r].x,gt[r].y); 01172 std::vector<line>::iterator lineit; 01173 bool visibility = true; 01174 for ( lineit = mapWall.begin(); lineit < mapWall.end(); lineit++ ){ 01175 if (intersect(ray, *lineit, crosspoint)){ 01176 visibility = false; 01177 break; 01178 } 01179 } 01180 01181 if (visibility){ 01182 float distx = gt[robot].x-gt[r].x; 01183 float disty = gt[robot].y-gt[r].y; 01184 float dist = sqrt(distx*distx+disty*disty); 01185 if (dist < maxDist){ 01186 res++; 01187 rbase[r]->setCaptured(); 01188 } 01189 } 01190 } 01191 return res; 01192 } 01193 01194 double simulatedModel::evaluateVMap(visualMap& vm) const{ 01195 double error =0; 01196 double suma=0; 01197 int N=vm.getNLandmarks(); 01198 for (int l = 0; l<N; l++){ 01199 landmark* m = vm.getLandmarkById(l); 01200 suma += sqrt( pow( landmarks[m->descriptor[0]].pos.getX() - m->pos.getX() ,2) + 01201 pow( landmarks[m->descriptor[0]].pos.getY() - m->pos.getY() ,2) + 01202 pow( landmarks[m->descriptor[0]].pos.getZ() - m->pos.getZ() ,2) ); 01203 delete m; 01204 } 01205 error = sqrt(suma/N); 01206 01207 return error; 01208 01209 } 01210 01211
1.7.6.1
