$search
00001 // 00002 // simple timing test of the nav fn planner 00003 // expects a cost map in maps/willow-full-0.05.pgm 00004 // 00005 00006 #include <navfn/navfn.h> 00007 #include <navfn/navwin.h> 00008 #include <sys/time.h> 00009 #include <stdio.h> 00010 #include <stdlib.h> 00011 #include <stdint.h> 00012 00013 using namespace navfn; 00014 00015 extern "C" { 00016 #include <stdio.h> 00017 // pgm.h is not very friendly with system headers... need to undef max() and min() afterwards 00018 #include <pgm.h> 00019 #undef max 00020 #undef min 00021 } 00022 00023 int goal[2]; 00024 int start[2]; 00025 00026 double get_ms() 00027 { 00028 struct timeval t0; 00029 gettimeofday(&t0,NULL); 00030 double ret = t0.tv_sec * 1000.0; 00031 ret += ((double)t0.tv_usec)*0.001; 00032 return ret; 00033 } 00034 00035 NavWin *nwin; 00036 00037 void 00038 dispPot(NavFn *nav) 00039 { 00040 nwin->drawPot(nav); 00041 Fl::check(); 00042 } 00043 00044 00045 // <raw> is true for ROS-generated raw cost maps 00046 COSTTYPE *readPGM(const char *fname, int *width, int *height, bool raw = false); 00047 00048 int main(int argc, char **argv) 00049 { 00050 int dispn = 0; 00051 00052 int res = 50; // 50 mm resolution 00053 double size = 40.0; // 40 m on a side 00054 int inc = 2*COST_NEUTRAL; // thin wavefront 00055 00056 // get resolution (mm) and perhaps size (m) 00057 if (argc > 1) 00058 res = atoi(argv[1]); 00059 00060 if (argc > 2) 00061 size = atoi(argv[2]); 00062 00063 if (argc > 3) 00064 inc = atoi(argv[3]); 00065 00066 if (argc > 4) 00067 dispn = atoi(argv[4]); 00068 00069 NavFn *nav; 00070 00071 // try reading in a file 00072 int sx,sy; 00073 COSTTYPE *cmap = NULL; 00074 // cmap = readPGM("maps/willow-full-0.05.pgm",&sx,&sy); 00075 // cmap = readPGM("maps/navfn_test1.pgm",&sx,&sy,true); 00076 // cmap = readPGM("initial_costmap_1165_945.pgm",&sx,&sy,true); 00077 // cmap = readPGM("initial_costmap_2332_1825.pgm",&sx,&sy,true); 00078 cmap = readPGM("planning_problems/failure/test.pgm",&sx,&sy,true); 00079 if (cmap) 00080 { 00081 nav = new NavFn(sx,sy); 00082 00083 // find goal 00084 goal[0] = sx - 20; // default 00085 COSTTYPE *cm = cmap + sy/6 * sx + sx - 10; 00086 for (int i=0; i<sx-20; i++, cm--) 00087 { 00088 if (*cm == COST_NEUTRAL) 00089 { 00090 goal[0] = sx-10-i; 00091 printf("[NavTest] Found goal at X = %d\n", sx - 10 -i); 00092 break; 00093 } 00094 } 00095 goal[1] = sy/6; 00096 00097 // find start 00098 start[0] = 20; // default 00099 cm = cmap + 5*sy/6 * sx + 10; 00100 for (int i=0; i<sx-20; i++, cm++) 00101 { 00102 if (*cm == COST_NEUTRAL) 00103 { 00104 start[0] = 10+i; 00105 printf("[NavTest] Found start at X = %d\n", start[0]); 00106 break; 00107 } 00108 } 00109 start[1] = 5*sy/6; 00110 00111 // start[0] = 1146; 00112 // start[1] = 1293; 00113 00114 start[0] = 625; 00115 start[1] = 1117; 00116 00117 goal[0] = 891; 00118 goal[1] = 1439; 00119 00120 } 00121 else 00122 { 00123 sx = (int)((.001 + size) / (res*.001)); 00124 sy = sx; 00125 nav = new NavFn(sx,sy); // size in pixels 00126 goal[0] = sx-10; 00127 goal[1] = sy/2; 00128 start[0] = 20; 00129 start[1] = sy/2; 00130 } 00131 00132 // display 00133 nwin = new NavWin(sx,sy,"Potential Field"); 00134 nwin->maxval = 2*sx*COST_NEUTRAL; 00135 Fl::visual(FL_RGB); 00136 nwin->show(); 00137 00138 00139 // set goal and robot poses 00140 int *gg = goal; 00141 nav->setGoal(gg); 00142 int *ss = start; 00143 nav->setStart(ss); 00144 00145 // set display function 00146 nav->display(dispPot,dispn); 00147 00148 00149 nav->priInc = inc; 00150 printf("[NavTest] priority increment: %d\n", inc); 00151 00152 #if 0 00153 // calculate the nav fn and path 00154 double t0 = get_ms(); 00155 // set up cost map from file, if it exists 00156 if (cmap) 00157 { 00158 nav->setCostmap(cmap); 00159 nav->setupNavFn(true); 00160 } 00161 else 00162 { 00163 nav->setupNavFn(false); 00164 nav->setObs(); // simple obstacles 00165 } 00166 //nav->propNavFnDijkstra(sx*sy/20); 00167 nav->propNavFnAstar(sx*sy/20); 00168 double t1 = get_ms(); 00169 00170 printf("Time for plan calculation: %d ms\n", (int)(t1-t0)); 00171 00172 // path 00173 nav->calcPath(4000); 00174 00175 #else 00176 double t0 = get_ms(); 00177 // set up cost map from file, if it exists 00178 if (cmap) 00179 { 00180 // nav->setCostMap(cmap); 00181 memcpy(nav->costarr,cmap,sx*sy); 00182 nav->setupNavFn(true); 00183 } 00184 else 00185 { 00186 nav->setupNavFn(false); 00187 nav->setObs(); // simple obstacles 00188 } 00189 double t1 = get_ms(); 00190 // nav->calcNavFnAstar(); 00191 nav->calcNavFnDijkstra(true); 00192 double t2 = get_ms(); 00193 printf("Setup: %d ms Plan: %d ms Total: %d ms\n", 00194 (int)(t1-t0), (int)(t2-t1), (int)(t2-t0)); 00195 #endif 00196 00197 // draw potential field 00198 float mmax = 0.0; 00199 float *pp = nav->potarr; 00200 int ntot = 0; 00201 for (int i=0; i<nav->ny*nav->nx; i++, pp++) 00202 { 00203 if (*pp < 10e7 && *pp > mmax) 00204 mmax = *pp; 00205 if (*pp > 10e7) 00206 ntot++; // number of uncalculated cells 00207 } 00208 printf("[NavFn] Cells not touched: %d/%d\n", ntot, nav->nx*nav->ny); 00209 nwin->maxval = 4*mmax/3; 00210 dispPot(nav); 00211 while (Fl::check()) {} 00212 00213 #if 0 00214 goal[1] = size-2; 00215 int k = nav->getCellIndex(gg); 00216 int st_nx = nav->st_nx; 00217 for (int i=0; i<900; i++, k--) 00218 { 00219 float npot = nav->potgrid[k]; 00220 printf("Pot: %0.1f\n", npot); 00221 printf("L: %0.1f R: %0.1f U: %0.1f D: %0.1f\n", 00222 nav->potgrid[k-1], nav->potgrid[k+1], nav->potgrid[k-st_nx], nav->potgrid[k+st_nx]); 00223 } 00224 #endif 00225 00226 return 0; 00227 } 00228 00229 00230 // read in a PGM file for obstacles 00231 // no expansion yet... 00232 00233 static int CS; 00234 00235 void 00236 setcostobs(COSTTYPE *cmap, int n, int w) 00237 { 00238 CS = 11; 00239 for (int i=-CS/2; i<CS/2; i++) 00240 { 00241 COSTTYPE *cm = i*w + &cmap[n]; 00242 for (int j=-CS/2; j<CS/2; j++) 00243 cm[j] = COST_NEUTRAL + 50; 00244 } 00245 CS = 7; 00246 for (int i=-CS/2; i<CS/2; i++) 00247 { 00248 COSTTYPE *cm = i*w + &cmap[n]; 00249 for (int j=-CS/2; j<CS/2; j++) 00250 cm[j] = COST_OBS; 00251 } 00252 } 00253 00254 void setcostunk(COSTTYPE *cmap, int n, int w) 00255 { 00256 cmap[n] = COST_OBS; 00257 } 00258 00259 #define unknown_gray 0xCC // seems to be the value of "unknown" in maps 00260 00261 COSTTYPE * 00262 readPGM(const char *fname, int *width, int *height, bool raw) 00263 { 00264 pm_init("navtest",0); 00265 00266 FILE *pgmfile; 00267 pgmfile = fopen(fname,"r"); 00268 if (!pgmfile) 00269 { 00270 printf("[NavTest] Can't find file %s\n", fname); 00271 return NULL; 00272 } 00273 00274 printf("[NavTest] Reading costmap file %s\n", fname); 00275 int ncols, nrows; 00276 gray maxval; 00277 int format; 00278 pgm_readpgminit(pgmfile, &ncols, &nrows, &maxval, &format); 00279 printf("[NavTest] Size: %d x %d\n", ncols, nrows); 00280 00281 // set up cost map 00282 COSTTYPE *cmap = (COSTTYPE *)malloc(ncols*nrows*sizeof(COSTTYPE)); 00283 if (!raw) 00284 for (int i=0; i<ncols*nrows; i++) 00285 cmap[i] = COST_NEUTRAL; 00286 00287 gray * row(pgm_allocrow(ncols)); 00288 int otot = 0; 00289 int utot = 0; 00290 int ftot = 0; 00291 for (int ii = 0; ii < nrows; ii++) { 00292 pgm_readpgmrow(pgmfile, row, ncols, maxval, format); 00293 if (raw) // raw costmap from ROS 00294 { 00295 for (int jj(ncols - 1); jj >= 0; --jj) 00296 { 00297 int v = row[jj]; 00298 cmap[ii*ncols+jj] = v; 00299 if (v >= COST_OBS_ROS) 00300 otot++; 00301 if (v == 0) 00302 ftot++; 00303 } 00304 } 00305 else 00306 { 00307 ftot = ncols*nrows; 00308 for (int jj(ncols - 1); jj >= 0; --jj) 00309 { 00310 if (row[jj] < unknown_gray && ii < nrows-7 && ii > 7) 00311 { 00312 setcostobs(cmap,ii*ncols+jj,ncols); 00313 otot++; 00314 ftot--; 00315 } 00316 #if 1 00317 else if (row[jj] <= unknown_gray) 00318 { 00319 setcostunk(cmap,ii*ncols+jj,ncols); 00320 utot++; 00321 ftot--; 00322 } 00323 #endif 00324 } 00325 } 00326 } 00327 printf("[NavTest] Found %d obstacle cells, %d free cells, %d unknown cells\n", otot, ftot, utot); 00328 pgm_freerow(row); 00329 *width = ncols; 00330 *height = nrows; 00331 return cmap; 00332 }