navtest.cpp
Go to the documentation of this file.
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("navfn_pathlong.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] = 350;
00115       start[1] = 400;
00116 
00117       goal[0] = 350;
00118       goal[1] = 450;
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 }


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein
autogenerated on Sat Dec 28 2013 17:14:13