navfn.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 //
00035 // Navigation function computation
00036 // Uses Dijkstra's method
00037 // Modified for Euclidean-distance computation
00038 // 
00039 // Path calculation uses no interpolation when pot field is at max in
00040 //   nearby cells
00041 //
00042 // Path calc has sanity check that it succeeded
00043 //
00044 
00045 
00046 #include <navfn/navfn.h>
00047 #include <ros/console.h>
00048 
00049 namespace navfn {
00050 
00051   //
00052   // function to perform nav fn calculation
00053   // keeps track of internal buffers, will be more efficient
00054   //   if the size of the environment does not change
00055   //
00056 
00057   int
00058     create_nav_plan_astar(COSTTYPE *costmap, int nx, int ny,
00059         int* goal, int* start,
00060         float *plan, int nplan)
00061     {
00062       static NavFn *nav = NULL;
00063 
00064       if (nav == NULL)
00065         nav = new NavFn(nx,ny);
00066 
00067       if (nav->nx != nx || nav->ny != ny) // check for compatibility with previous call
00068       {
00069         delete nav;
00070         nav = new NavFn(nx,ny);      
00071       }
00072 
00073       nav->setGoal(goal);
00074       nav->setStart(start);
00075 
00076       nav->costarr = costmap;
00077       nav->setupNavFn(true);
00078 
00079       // calculate the nav fn and path
00080       nav->priInc = 2*COST_NEUTRAL;
00081       nav->propNavFnAstar(std::max(nx*ny/20,nx+ny));
00082 
00083       // path
00084       int len = nav->calcPath(nplan);
00085 
00086       if (len > 0)                      // found plan
00087         ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
00088       else
00089         ROS_DEBUG("[NavFn] No path found\n");
00090 
00091       if (len > 0)
00092       {
00093         for (int i=0; i<len; i++)
00094         {
00095           plan[i*2] = nav->pathx[i];
00096           plan[i*2+1] = nav->pathy[i];
00097         }
00098       }
00099 
00100       return len;
00101     }
00102 
00103 
00104 
00105 
00106   //
00107   // create nav fn buffers 
00108   //
00109 
00110   NavFn::NavFn(int xs, int ys)
00111   {  
00112     // create cell arrays
00113     costarr = NULL;
00114     potarr = NULL;
00115     pending = NULL;
00116     gradx = grady = NULL;
00117     setNavArr(xs,ys);
00118 
00119     // priority buffers
00120     pb1 = new int[PRIORITYBUFSIZE];
00121     pb2 = new int[PRIORITYBUFSIZE];
00122     pb3 = new int[PRIORITYBUFSIZE];
00123 
00124     // for Dijkstra (breadth-first), set to COST_NEUTRAL
00125     // for A* (best-first), set to COST_NEUTRAL
00126     priInc = 2*COST_NEUTRAL;    
00127 
00128     // goal and start
00129     goal[0] = goal[1] = 0;
00130     start[0] = start[1] = 0;
00131 
00132     // display function
00133     displayFn = NULL;
00134     displayInt = 0;
00135 
00136     // path buffers
00137     npathbuf = npath = 0;
00138     pathx = pathy = NULL;
00139     pathStep = 0.5;
00140   }
00141 
00142 
00143   NavFn::~NavFn()
00144   {
00145     if(costarr)
00146       delete[] costarr;
00147     if(potarr)
00148       delete[] potarr;
00149     if(pending)
00150       delete[] pending;
00151     if(gradx)
00152       delete[] gradx;
00153     if(grady)
00154       delete[] grady;
00155     if(pathx)
00156       delete[] pathx;
00157     if(pathy)
00158       delete[] pathy;
00159     if(pb1)
00160       delete[] pb1;
00161     if(pb2)
00162       delete[] pb2;
00163     if(pb3)
00164       delete[] pb3;
00165   }
00166 
00167 
00168   //
00169   // set goal, start positions for the nav fn
00170   //
00171 
00172   void
00173     NavFn::setGoal(int *g)
00174     {
00175       goal[0] = g[0];
00176       goal[1] = g[1];
00177       ROS_DEBUG("[NavFn] Setting goal to %d,%d\n", goal[0], goal[1]);
00178     }
00179 
00180   void
00181     NavFn::setStart(int *g)
00182     {
00183       start[0] = g[0];
00184       start[1] = g[1];
00185       ROS_DEBUG("[NavFn] Setting start to %d,%d\n", start[0], start[1]);
00186     }
00187 
00188   //
00189   // Set/Reset map size
00190   //
00191 
00192   void
00193     NavFn::setNavArr(int xs, int ys)
00194     {
00195       ROS_DEBUG("[NavFn] Array is %d x %d\n", xs, ys);
00196 
00197       nx = xs;
00198       ny = ys;
00199       ns = nx*ny;
00200 
00201       if(costarr)
00202         delete[] costarr;
00203       if(potarr)
00204         delete[] potarr;
00205       if(pending)
00206         delete[] pending;
00207 
00208       if(gradx)
00209         delete[] gradx;
00210       if(grady)
00211         delete[] grady;
00212 
00213       costarr = new COSTTYPE[ns]; // cost array, 2d config space
00214       memset(costarr, 0, ns*sizeof(COSTTYPE));
00215       potarr = new float[ns];   // navigation potential array
00216       pending = new bool[ns];
00217       memset(pending, 0, ns*sizeof(bool));
00218       gradx = new float[ns];
00219       grady = new float[ns];
00220     }
00221 
00222 
00223   //
00224   // set up cost array, usually from ROS
00225   //
00226 
00227   void
00228     NavFn::setCostmap(const COSTTYPE *cmap, bool isROS, bool allow_unknown)
00229     {
00230       COSTTYPE *cm = costarr;
00231       if (isROS)                        // ROS-type cost array
00232       {
00233         for (int i=0; i<ny; i++)
00234         {
00235           int k=i*nx;
00236           for (int j=0; j<nx; j++, k++, cmap++, cm++)
00237           {
00238             // This transforms the incoming cost values:
00239             // COST_OBS                 -> COST_OBS (incoming "lethal obstacle")
00240             // COST_OBS_ROS             -> COST_OBS (incoming "inscribed inflated obstacle")
00241             // values in range 0 to 252 -> values from COST_NEUTRAL to COST_OBS_ROS.
00242             *cm = COST_OBS;
00243             int v = *cmap;
00244             if (v < COST_OBS_ROS)
00245             {
00246               v = COST_NEUTRAL+COST_FACTOR*v;
00247               if (v >= COST_OBS)
00248                 v = COST_OBS-1;
00249               *cm = v;
00250             }
00251             else if(v == COST_UNKNOWN_ROS && allow_unknown)
00252             {
00253               v = COST_OBS-1;
00254               *cm = v;
00255             }
00256           }
00257         }
00258       }
00259 
00260       else                              // not a ROS map, just a PGM
00261       {
00262         for (int i=0; i<ny; i++)
00263         {
00264           int k=i*nx;
00265           for (int j=0; j<nx; j++, k++, cmap++, cm++)
00266           {
00267             *cm = COST_OBS;
00268             if (i<7 || i > ny-8 || j<7 || j > nx-8)
00269               continue; // don't do borders
00270             int v = *cmap;
00271             if (v < COST_OBS_ROS)
00272             {
00273               v = COST_NEUTRAL+COST_FACTOR*v;
00274               if (v >= COST_OBS)
00275                 v = COST_OBS-1;
00276               *cm = v;
00277             }
00278             else if(v == COST_UNKNOWN_ROS)
00279             {
00280               v = COST_OBS-1;
00281               *cm = v;
00282             }
00283           }
00284         }
00285 
00286       }
00287     }
00288 
00289   bool
00290     NavFn::calcNavFnDijkstra(bool atStart)
00291     {
00292 #if 0
00293       static char costmap_filename[1000];
00294       static int file_number = 0;
00295       snprintf( costmap_filename, 1000, "navfn-dijkstra-costmap-%04d", file_number++ );
00296       savemap( costmap_filename );
00297 #endif
00298       setupNavFn(true);
00299 
00300       // calculate the nav fn and path
00301       propNavFnDijkstra(std::max(nx*ny/20,nx+ny),atStart);
00302 
00303       // path
00304       int len = calcPath(nx*ny/2);
00305 
00306       if (len > 0)                      // found plan
00307       {
00308         ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
00309         return true;
00310       }
00311       else
00312       {
00313         ROS_DEBUG("[NavFn] No path found\n");
00314         return false;
00315       }
00316 
00317     }
00318 
00319 
00320   //
00321   // calculate navigation function, given a costmap, goal, and start
00322   //
00323 
00324   bool
00325     NavFn::calcNavFnAstar()
00326     {
00327       setupNavFn(true);
00328 
00329       // calculate the nav fn and path
00330       propNavFnAstar(std::max(nx*ny/20,nx+ny));
00331 
00332       // path
00333       int len = calcPath(nx*4);
00334 
00335       if (len > 0)                      // found plan
00336       {
00337         ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
00338         return true;
00339       }
00340       else
00341       {
00342         ROS_DEBUG("[NavFn] No path found\n");
00343         return false;
00344       }
00345     }
00346 
00347 
00348   //
00349   // returning values
00350   //
00351 
00352   float *NavFn::getPathX() { return pathx; }
00353   float *NavFn::getPathY() { return pathy; }
00354   int    NavFn::getPathLen() { return npath; }
00355 
00356 
00357   //
00358   // simple obstacle setup for tests
00359   //
00360 
00361   void
00362     NavFn::setObs()
00363     {
00364 #if 0
00365       // set up a simple obstacle
00366       ROS_INFO("[NavFn] Setting simple obstacle\n");
00367       int xx = nx/3;
00368       for (int i=ny/3; i<ny; i++)
00369         costarr[i*nx + xx] = COST_OBS;
00370       xx = 2*nx/3;
00371       for (int i=ny/3; i<ny; i++)
00372         costarr[i*nx + xx] = COST_OBS;
00373 
00374       xx = nx/4;
00375       for (int i=20; i<ny-ny/3; i++)
00376         costarr[i*nx + xx] = COST_OBS;
00377       xx = nx/2;
00378       for (int i=20; i<ny-ny/3; i++)
00379         costarr[i*nx + xx] = COST_OBS;
00380       xx = 3*nx/4;
00381       for (int i=20; i<ny-ny/3; i++)
00382         costarr[i*nx + xx] = COST_OBS;
00383 #endif
00384     }
00385 
00386 
00387   // inserting onto the priority blocks
00388 #define push_cur(n)  { if (n>=0 && n<ns && !pending[n] && \
00389     costarr[n]<COST_OBS && curPe<PRIORITYBUFSIZE) \
00390   { curP[curPe++]=n; pending[n]=true; }}
00391 #define push_next(n) { if (n>=0 && n<ns && !pending[n] && \
00392     costarr[n]<COST_OBS && nextPe<PRIORITYBUFSIZE) \
00393   { nextP[nextPe++]=n; pending[n]=true; }}
00394 #define push_over(n) { if (n>=0 && n<ns && !pending[n] && \
00395     costarr[n]<COST_OBS && overPe<PRIORITYBUFSIZE) \
00396   { overP[overPe++]=n; pending[n]=true; }}
00397 
00398 
00399   // Set up navigation potential arrays for new propagation
00400 
00401   void
00402     NavFn::setupNavFn(bool keepit)
00403     {
00404       // reset values in propagation arrays
00405       for (int i=0; i<ns; i++)
00406       {
00407         potarr[i] = POT_HIGH;
00408         if (!keepit) costarr[i] = COST_NEUTRAL;
00409         gradx[i] = grady[i] = 0.0;
00410       }
00411 
00412       // outer bounds of cost array
00413       COSTTYPE *pc;
00414       pc = costarr;
00415       for (int i=0; i<nx; i++)
00416         *pc++ = COST_OBS;
00417       pc = costarr + (ny-1)*nx;
00418       for (int i=0; i<nx; i++)
00419         *pc++ = COST_OBS;
00420       pc = costarr;
00421       for (int i=0; i<ny; i++, pc+=nx)
00422         *pc = COST_OBS;
00423       pc = costarr + nx - 1;
00424       for (int i=0; i<ny; i++, pc+=nx)
00425         *pc = COST_OBS;
00426 
00427       // priority buffers
00428       curT = COST_OBS;
00429       curP = pb1; 
00430       curPe = 0;
00431       nextP = pb2;
00432       nextPe = 0;
00433       overP = pb3;
00434       overPe = 0;
00435       memset(pending, 0, ns*sizeof(bool));
00436 
00437       // set goal
00438       int k = goal[0] + goal[1]*nx;
00439       initCost(k,0);
00440 
00441       // find # of obstacle cells
00442       pc = costarr;
00443       int ntot = 0;
00444       for (int i=0; i<ns; i++, pc++)
00445       {
00446         if (*pc >= COST_OBS)
00447           ntot++;                       // number of cells that are obstacles
00448       }
00449       nobs = ntot;
00450     }
00451 
00452 
00453   // initialize a goal-type cost for starting propagation
00454 
00455   void
00456     NavFn::initCost(int k, float v)
00457     {
00458       potarr[k] = v;
00459       push_cur(k+1);
00460       push_cur(k-1);
00461       push_cur(k-nx);
00462       push_cur(k+nx);
00463     }
00464 
00465 
00466   // 
00467   // Critical function: calculate updated potential value of a cell,
00468   //   given its neighbors' values
00469   // Planar-wave update calculation from two lowest neighbors in a 4-grid
00470   // Quadratic approximation to the interpolated value 
00471   // No checking of bounds here, this function should be fast
00472   //
00473 
00474 #define INVSQRT2 0.707106781
00475 
00476   inline void
00477     NavFn::updateCell(int n)
00478     {
00479       // get neighbors
00480       float u,d,l,r;
00481       l = potarr[n-1];
00482       r = potarr[n+1];          
00483       u = potarr[n-nx];
00484       d = potarr[n+nx];
00485       //  ROS_INFO("[Update] c: %0.1f  l: %0.1f  r: %0.1f  u: %0.1f  d: %0.1f\n", 
00486       //         potarr[n], l, r, u, d);
00487       //  ROS_INFO("[Update] cost: %d\n", costarr[n]);
00488 
00489       // find lowest, and its lowest neighbor
00490       float ta, tc;
00491       if (l<r) tc=l; else tc=r;
00492       if (u<d) ta=u; else ta=d;
00493 
00494       // do planar wave update
00495       if (costarr[n] < COST_OBS)        // don't propagate into obstacles
00496       {
00497         float hf = (float)costarr[n]; // traversability factor
00498         float dc = tc-ta;               // relative cost between ta,tc
00499         if (dc < 0)             // ta is lowest
00500         {
00501           dc = -dc;
00502           ta = tc;
00503         }
00504 
00505         // calculate new potential
00506         float pot;
00507         if (dc >= hf)           // if too large, use ta-only update
00508           pot = ta+hf;
00509         else                    // two-neighbor interpolation update
00510         {
00511           // use quadratic approximation
00512           // might speed this up through table lookup, but still have to 
00513           //   do the divide
00514           float d = dc/hf;
00515           float v = -0.2301*d*d + 0.5307*d + 0.7040;
00516           pot = ta + hf*v;
00517         }
00518 
00519         //      ROS_INFO("[Update] new pot: %d\n", costarr[n]);
00520 
00521         // now add affected neighbors to priority blocks
00522         if (pot < potarr[n])
00523         {
00524           float le = INVSQRT2*(float)costarr[n-1];
00525           float re = INVSQRT2*(float)costarr[n+1];
00526           float ue = INVSQRT2*(float)costarr[n-nx];
00527           float de = INVSQRT2*(float)costarr[n+nx];
00528           potarr[n] = pot;
00529           if (pot < curT)       // low-cost buffer block 
00530           {
00531             if (l > pot+le) push_next(n-1);
00532             if (r > pot+re) push_next(n+1);
00533             if (u > pot+ue) push_next(n-nx);
00534             if (d > pot+de) push_next(n+nx);
00535           }
00536           else                  // overflow block
00537           {
00538             if (l > pot+le) push_over(n-1);
00539             if (r > pot+re) push_over(n+1);
00540             if (u > pot+ue) push_over(n-nx);
00541             if (d > pot+de) push_over(n+nx);
00542           }
00543         }
00544 
00545       }
00546 
00547     }
00548 
00549 
00550   //
00551   // Use A* method for setting priorities
00552   // Critical function: calculate updated potential value of a cell,
00553   //   given its neighbors' values
00554   // Planar-wave update calculation from two lowest neighbors in a 4-grid
00555   // Quadratic approximation to the interpolated value 
00556   // No checking of bounds here, this function should be fast
00557   //
00558 
00559 #define INVSQRT2 0.707106781
00560 
00561   inline void
00562     NavFn::updateCellAstar(int n)
00563     {
00564       // get neighbors
00565       float u,d,l,r;
00566       l = potarr[n-1];
00567       r = potarr[n+1];          
00568       u = potarr[n-nx];
00569       d = potarr[n+nx];
00570       //ROS_INFO("[Update] c: %0.1f  l: %0.1f  r: %0.1f  u: %0.1f  d: %0.1f\n", 
00571       //         potarr[n], l, r, u, d);
00572       // ROS_INFO("[Update] cost of %d: %d\n", n, costarr[n]);
00573 
00574       // find lowest, and its lowest neighbor
00575       float ta, tc;
00576       if (l<r) tc=l; else tc=r;
00577       if (u<d) ta=u; else ta=d;
00578 
00579       // do planar wave update
00580       if (costarr[n] < COST_OBS)        // don't propagate into obstacles
00581       {
00582         float hf = (float)costarr[n]; // traversability factor
00583         float dc = tc-ta;               // relative cost between ta,tc
00584         if (dc < 0)             // ta is lowest
00585         {
00586           dc = -dc;
00587           ta = tc;
00588         }
00589 
00590         // calculate new potential
00591         float pot;
00592         if (dc >= hf)           // if too large, use ta-only update
00593           pot = ta+hf;
00594         else                    // two-neighbor interpolation update
00595         {
00596           // use quadratic approximation
00597           // might speed this up through table lookup, but still have to 
00598           //   do the divide
00599           float d = dc/hf;
00600           float v = -0.2301*d*d + 0.5307*d + 0.7040;
00601           pot = ta + hf*v;
00602         }
00603 
00604         //ROS_INFO("[Update] new pot: %d\n", costarr[n]);
00605 
00606         // now add affected neighbors to priority blocks
00607         if (pot < potarr[n])
00608         {
00609           float le = INVSQRT2*(float)costarr[n-1];
00610           float re = INVSQRT2*(float)costarr[n+1];
00611           float ue = INVSQRT2*(float)costarr[n-nx];
00612           float de = INVSQRT2*(float)costarr[n+nx];
00613 
00614           // calculate distance
00615           int x = n%nx;
00616           int y = n/nx;
00617           float dist = hypot(x-start[0], y-start[1])*(float)COST_NEUTRAL;
00618 
00619           potarr[n] = pot;
00620           pot += dist;
00621           if (pot < curT)       // low-cost buffer block 
00622           {
00623             if (l > pot+le) push_next(n-1);
00624             if (r > pot+re) push_next(n+1);
00625             if (u > pot+ue) push_next(n-nx);
00626             if (d > pot+de) push_next(n+nx);
00627           }
00628           else
00629           {
00630             if (l > pot+le) push_over(n-1);
00631             if (r > pot+re) push_over(n+1);
00632             if (u > pot+ue) push_over(n-nx);
00633             if (d > pot+de) push_over(n+nx);
00634           }
00635         }
00636 
00637       }
00638 
00639     }
00640 
00641 
00642 
00643   //
00644   // main propagation function
00645   // Dijkstra method, breadth-first
00646   // runs for a specified number of cycles,
00647   //   or until it runs out of cells to update,
00648   //   or until the Start cell is found (atStart = true)
00649   //
00650 
00651   bool
00652     NavFn::propNavFnDijkstra(int cycles, bool atStart)  
00653     {
00654       int nwv = 0;                      // max priority block size
00655       int nc = 0;                       // number of cells put into priority blocks
00656       int cycle = 0;            // which cycle we're on
00657 
00658       // set up start cell
00659       int startCell = start[1]*nx + start[0];
00660 
00661       for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
00662       {
00663         // 
00664         if (curPe == 0 && nextPe == 0) // priority blocks empty
00665           break;
00666 
00667         // stats
00668         nc += curPe;
00669         if (curPe > nwv)
00670           nwv = curPe;
00671 
00672         // reset pending flags on current priority buffer
00673         int *pb = curP;
00674         int i = curPe;                  
00675         while (i-- > 0)         
00676           pending[*(pb++)] = false;
00677 
00678         // process current priority buffer
00679         pb = curP; 
00680         i = curPe;
00681         while (i-- > 0)         
00682           updateCell(*pb++);
00683 
00684         if (displayInt > 0 &&  (cycle % displayInt) == 0)
00685           displayFn(this);
00686 
00687         // swap priority blocks curP <=> nextP
00688         curPe = nextPe;
00689         nextPe = 0;
00690         pb = curP;              // swap buffers
00691         curP = nextP;
00692         nextP = pb;
00693 
00694         // see if we're done with this priority level
00695         if (curPe == 0)
00696         {
00697           curT += priInc;       // increment priority threshold
00698           curPe = overPe;       // set current to overflow block
00699           overPe = 0;
00700           pb = curP;            // swap buffers
00701           curP = overP;
00702           overP = pb;
00703         }
00704 
00705         // check if we've hit the Start cell
00706         if (atStart)
00707           if (potarr[startCell] < POT_HIGH)
00708             break;
00709       }
00710 
00711       ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n", 
00712           cycle,nc,(int)((nc*100.0)/(ns-nobs)),nwv);
00713 
00714       if (cycle < cycles) return true; // finished up here
00715       else return false;
00716     }
00717 
00718 
00719   //
00720   // main propagation function
00721   // A* method, best-first
00722   // uses Euclidean distance heuristic
00723   // runs for a specified number of cycles,
00724   //   or until it runs out of cells to update,
00725   //   or until the Start cell is found (atStart = true)
00726   //
00727 
00728   bool
00729     NavFn::propNavFnAstar(int cycles)   
00730     {
00731       int nwv = 0;                      // max priority block size
00732       int nc = 0;                       // number of cells put into priority blocks
00733       int cycle = 0;            // which cycle we're on
00734 
00735       // set initial threshold, based on distance
00736       float dist = hypot(goal[0]-start[0], goal[1]-start[1])*(float)COST_NEUTRAL;
00737       curT = dist + curT;
00738 
00739       // set up start cell
00740       int startCell = start[1]*nx + start[0];
00741 
00742       // do main cycle
00743       for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
00744       {
00745         // 
00746         if (curPe == 0 && nextPe == 0) // priority blocks empty
00747           break;
00748 
00749         // stats
00750         nc += curPe;
00751         if (curPe > nwv)
00752           nwv = curPe;
00753 
00754         // reset pending flags on current priority buffer
00755         int *pb = curP;
00756         int i = curPe;                  
00757         while (i-- > 0)         
00758           pending[*(pb++)] = false;
00759 
00760         // process current priority buffer
00761         pb = curP; 
00762         i = curPe;
00763         while (i-- > 0)         
00764           updateCellAstar(*pb++);
00765 
00766         if (displayInt > 0 &&  (cycle % displayInt) == 0)
00767           displayFn(this);
00768 
00769         // swap priority blocks curP <=> nextP
00770         curPe = nextPe;
00771         nextPe = 0;
00772         pb = curP;              // swap buffers
00773         curP = nextP;
00774         nextP = pb;
00775 
00776         // see if we're done with this priority level
00777         if (curPe == 0)
00778         {
00779           curT += priInc;       // increment priority threshold
00780           curPe = overPe;       // set current to overflow block
00781           overPe = 0;
00782           pb = curP;            // swap buffers
00783           curP = overP;
00784           overP = pb;
00785         }
00786 
00787         // check if we've hit the Start cell
00788         if (potarr[startCell] < POT_HIGH)
00789           break;
00790 
00791       }
00792 
00793       last_path_cost_ = potarr[startCell];
00794 
00795       ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n", 
00796           cycle,nc,(int)((nc*100.0)/(ns-nobs)),nwv);
00797 
00798 
00799       if (potarr[startCell] < POT_HIGH) return true; // finished up here
00800       else return false;
00801     }
00802 
00803 
00804   float NavFn::getLastPathCost()
00805   {
00806     return last_path_cost_;
00807   }
00808 
00809 
00810   //
00811   // Path construction
00812   // Find gradient at array points, interpolate path
00813   // Use step size of pathStep, usually 0.5 pixel
00814   //
00815   // Some sanity checks:
00816   //  1. Stuck at same index position
00817   //  2. Doesn't get near goal
00818   //  3. Surrounded by high potentials
00819   //
00820 
00821   int
00822     NavFn::calcPath(int n, int *st)
00823     {
00824       // test write
00825       //savemap("test");
00826 
00827       // check path arrays
00828       if (npathbuf < n)
00829       {
00830         if (pathx) delete [] pathx;
00831         if (pathy) delete [] pathy;
00832         pathx = new float[n];
00833         pathy = new float[n];
00834         npathbuf = n;
00835       }
00836 
00837       // set up start position at cell
00838       // st is always upper left corner for 4-point bilinear interpolation 
00839       if (st == NULL) st = start;
00840       int stc = st[1]*nx + st[0];
00841 
00842       // set up offset
00843       float dx=0;
00844       float dy=0;
00845       npath = 0;
00846 
00847       // go for <n> cycles at most
00848       for (int i=0; i<n; i++)
00849       {
00850         // check if near goal
00851         int nearest_point=std::max(0,std::min(nx*ny-1,stc+(int)round(dx)+(int)(nx*round(dy))));
00852         if (potarr[nearest_point] < COST_NEUTRAL)
00853         {
00854           pathx[npath] = (float)goal[0];
00855           pathy[npath] = (float)goal[1];
00856           return ++npath;       // done!
00857         }
00858 
00859         if (stc < nx || stc > ns-nx) // would be out of bounds
00860         {
00861           ROS_DEBUG("[PathCalc] Out of bounds");
00862           return 0;
00863         }
00864 
00865         // add to path
00866         pathx[npath] = stc%nx + dx;
00867         pathy[npath] = stc/nx + dy;
00868         npath++;
00869 
00870         bool oscillation_detected = false;
00871         if( npath > 2 &&
00872             pathx[npath-1] == pathx[npath-3] &&
00873             pathy[npath-1] == pathy[npath-3] )
00874         {
00875           ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
00876           oscillation_detected = true;
00877         }
00878 
00879         int stcnx = stc+nx;
00880         int stcpx = stc-nx;
00881 
00882         // check for potentials at eight positions near cell
00883         if (potarr[stc] >= POT_HIGH ||
00884             potarr[stc+1] >= POT_HIGH ||
00885             potarr[stc-1] >= POT_HIGH ||
00886             potarr[stcnx] >= POT_HIGH ||
00887             potarr[stcnx+1] >= POT_HIGH ||
00888             potarr[stcnx-1] >= POT_HIGH ||
00889             potarr[stcpx] >= POT_HIGH ||
00890             potarr[stcpx+1] >= POT_HIGH ||
00891             potarr[stcpx-1] >= POT_HIGH ||
00892             oscillation_detected)
00893         {
00894           ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath);
00895           // check eight neighbors to find the lowest
00896           int minc = stc;
00897           int minp = potarr[stc];
00898           int st = stcpx - 1;
00899           if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
00900           st++;
00901           if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
00902           st++;
00903           if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
00904           st = stc-1;
00905           if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
00906           st = stc+1;
00907           if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
00908           st = stcnx-1;
00909           if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
00910           st++;
00911           if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
00912           st++;
00913           if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
00914           stc = minc;
00915           dx = 0;
00916           dy = 0;
00917 
00918           ROS_DEBUG("[Path] Pot: %0.1f  pos: %0.1f,%0.1f",
00919               potarr[stc], pathx[npath-1], pathy[npath-1]);
00920 
00921           if (potarr[stc] >= POT_HIGH)
00922           {
00923             ROS_DEBUG("[PathCalc] No path found, high potential");
00924             //savemap("navfn_highpot");
00925             return 0;
00926           }
00927         }
00928 
00929         // have a good gradient here
00930         else                    
00931         {
00932 
00933           // get grad at four positions near cell
00934           gradCell(stc);
00935           gradCell(stc+1);
00936           gradCell(stcnx);
00937           gradCell(stcnx+1);
00938 
00939 
00940           // get interpolated gradient
00941           float x1 = (1.0-dx)*gradx[stc] + dx*gradx[stc+1];
00942           float x2 = (1.0-dx)*gradx[stcnx] + dx*gradx[stcnx+1];
00943           float x = (1.0-dy)*x1 + dy*x2; // interpolated x
00944           float y1 = (1.0-dx)*grady[stc] + dx*grady[stc+1];
00945           float y2 = (1.0-dx)*grady[stcnx] + dx*grady[stcnx+1];
00946           float y = (1.0-dy)*y1 + dy*y2; // interpolated y
00947 
00948           // show gradients
00949           ROS_DEBUG("[Path] %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f  %0.2f,%0.2f; final x=%.3f, y=%.3f\n",
00950                     gradx[stc], grady[stc], gradx[stc+1], grady[stc+1], 
00951                     gradx[stcnx], grady[stcnx], gradx[stcnx+1], grady[stcnx+1],
00952                     x, y);
00953 
00954           // check for zero gradient, failed
00955           if (x == 0.0 && y == 0.0)
00956           {
00957             ROS_DEBUG("[PathCalc] Zero gradient");        
00958             return 0;
00959           }
00960 
00961           // move in the right direction
00962           float ss = pathStep/hypot(x, y);
00963           dx += x*ss;
00964           dy += y*ss;
00965 
00966           // check for overflow
00967           if (dx > 1.0) { stc++; dx -= 1.0; }
00968           if (dx < -1.0) { stc--; dx += 1.0; }
00969           if (dy > 1.0) { stc+=nx; dy -= 1.0; }
00970           if (dy < -1.0) { stc-=nx; dy += 1.0; }
00971 
00972         }
00973 
00974         //      ROS_INFO("[Path] Pot: %0.1f  grad: %0.1f,%0.1f  pos: %0.1f,%0.1f\n",
00975         //           potarr[stc], x, y, pathx[npath-1], pathy[npath-1]);
00976       }
00977 
00978       //  return npath;                 // out of cycles, return failure
00979       ROS_DEBUG("[PathCalc] No path found, path too long");
00980       //savemap("navfn_pathlong");
00981       return 0;                 // out of cycles, return failure
00982     }
00983 
00984 
00985   //
00986   // gradient calculations
00987   //
00988 
00989   // calculate gradient at a cell
00990   // positive value are to the right and down
00991   float                         
00992     NavFn::gradCell(int n)
00993     {
00994       if (gradx[n]+grady[n] > 0.0)      // check this cell
00995         return 1.0;                     
00996 
00997       if (n < nx || n > ns-nx)  // would be out of bounds
00998         return 0.0;
00999 
01000       float cv = potarr[n];
01001       float dx = 0.0;
01002       float dy = 0.0;
01003 
01004       // check for in an obstacle
01005       if (cv >= POT_HIGH) 
01006       {
01007         if (potarr[n-1] < POT_HIGH)
01008           dx = -COST_OBS;
01009         else if (potarr[n+1] < POT_HIGH)
01010           dx = COST_OBS;
01011 
01012         if (potarr[n-nx] < POT_HIGH)
01013           dy = -COST_OBS;
01014         else if (potarr[nx+1] < POT_HIGH)
01015           dy = COST_OBS;
01016       }
01017 
01018       else                              // not in an obstacle
01019       {
01020         // dx calc, average to sides
01021         if (potarr[n-1] < POT_HIGH)
01022           dx += potarr[n-1]- cv;        
01023         if (potarr[n+1] < POT_HIGH)
01024           dx += cv - potarr[n+1]; 
01025 
01026         // dy calc, average to sides
01027         if (potarr[n-nx] < POT_HIGH)
01028           dy += potarr[n-nx]- cv;       
01029         if (potarr[n+nx] < POT_HIGH)
01030           dy += cv - potarr[n+nx]; 
01031       }
01032 
01033       // normalize
01034       float norm = hypot(dx, dy);
01035       if (norm > 0)
01036       {
01037         norm = 1.0/norm;
01038         gradx[n] = norm*dx;
01039         grady[n] = norm*dy;
01040       }
01041       return norm;
01042     }
01043 
01044 
01045   //
01046   // display function setup
01047   // <n> is the number of cycles to wait before displaying,
01048   //     use 0 to turn it off
01049 
01050   void
01051     NavFn::display(void fn(NavFn *nav), int n)
01052     {
01053       displayFn = fn;
01054       displayInt = n;
01055     }
01056 
01057 
01058   //
01059   // debug writes
01060   // saves costmap and start/goal
01061   //
01062 
01063   void 
01064     NavFn::savemap(const char *fname)
01065     {
01066       char fn[4096];
01067 
01068       ROS_DEBUG("[NavFn] Saving costmap and start/goal points");
01069       // write start and goal points
01070       sprintf(fn,"%s.txt",fname);
01071       FILE *fp = fopen(fn,"w");
01072       if (!fp)
01073       {
01074         ROS_WARN("Can't open file %s", fn);
01075         return;
01076       }
01077       fprintf(fp,"Goal: %d %d\nStart: %d %d\n",goal[0],goal[1],start[0],start[1]);
01078       fclose(fp);
01079 
01080       // write cost array
01081       if (!costarr) return;
01082       sprintf(fn,"%s.pgm",fname);
01083       fp = fopen(fn,"wb");
01084       if (!fp)
01085       {
01086         ROS_WARN("Can't open file %s", fn);
01087         return;
01088       }
01089       fprintf(fp,"P5\n%d\n%d\n%d\n", nx, ny, 0xff);
01090       fwrite(costarr,1,nx*ny,fp);
01091       fclose(fp);
01092     }
01093 };


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