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


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:27