navfn.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 //
35 // Navigation function computation
36 // Uses Dijkstra's method
37 // Modified for Euclidean-distance computation
38 //
39 // Path calculation uses no interpolation when pot field is at max in
40 // nearby cells
41 //
42 // Path calc has sanity check that it succeeded
43 //
44 
45 
46 #include <navfn/navfn.h>
47 #include <ros/console.h>
48 
49 namespace navfn {
50 
51  //
52  // function to perform nav fn calculation
53  // keeps track of internal buffers, will be more efficient
54  // if the size of the environment does not change
55  //
56 
57  int
58  create_nav_plan_astar(COSTTYPE *costmap, int nx, int ny,
59  int* goal, int* start,
60  float *plan, int nplan)
61  {
62  static NavFn *nav = NULL;
63 
64  if (nav == NULL)
65  nav = new NavFn(nx,ny);
66 
67  if (nav->nx != nx || nav->ny != ny) // check for compatibility with previous call
68  {
69  delete nav;
70  nav = new NavFn(nx,ny);
71  }
72 
73  nav->setGoal(goal);
74  nav->setStart(start);
75 
76  nav->costarr = costmap;
77  nav->setupNavFn(true);
78 
79  // calculate the nav fn and path
80  nav->priInc = 2*COST_NEUTRAL;
81  nav->propNavFnAstar(std::max(nx*ny/20,nx+ny));
82 
83  // path
84  int len = nav->calcPath(nplan);
85 
86  if (len > 0) // found plan
87  ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
88  else
89  ROS_DEBUG("[NavFn] No path found\n");
90 
91  if (len > 0)
92  {
93  for (int i=0; i<len; i++)
94  {
95  plan[i*2] = nav->pathx[i];
96  plan[i*2+1] = nav->pathy[i];
97  }
98  }
99 
100  return len;
101  }
102 
103 
104 
105 
106  //
107  // create nav fn buffers
108  //
109 
110  NavFn::NavFn(int xs, int ys)
111  {
112  // create cell arrays
113  costarr = NULL;
114  potarr = NULL;
115  pending = NULL;
116  gradx = grady = NULL;
117  setNavArr(xs,ys);
118 
119  // priority buffers
120  pb1 = new int[PRIORITYBUFSIZE];
121  pb2 = new int[PRIORITYBUFSIZE];
122  pb3 = new int[PRIORITYBUFSIZE];
123 
124  // for Dijkstra (breadth-first), set to COST_NEUTRAL
125  // for A* (best-first), set to COST_NEUTRAL
126  priInc = 2*COST_NEUTRAL;
127 
128  // goal and start
129  goal[0] = goal[1] = 0;
130  start[0] = start[1] = 0;
131 
132  // display function
133  displayFn = NULL;
134  displayInt = 0;
135 
136  // path buffers
137  npathbuf = npath = 0;
138  pathx = pathy = NULL;
139  pathStep = 0.5;
140  }
141 
142 
144  {
145  if(costarr)
146  delete[] costarr;
147  if(potarr)
148  delete[] potarr;
149  if(pending)
150  delete[] pending;
151  if(gradx)
152  delete[] gradx;
153  if(grady)
154  delete[] grady;
155  if(pathx)
156  delete[] pathx;
157  if(pathy)
158  delete[] pathy;
159  if(pb1)
160  delete[] pb1;
161  if(pb2)
162  delete[] pb2;
163  if(pb3)
164  delete[] pb3;
165  }
166 
167 
168  //
169  // set goal, start positions for the nav fn
170  //
171 
172  void
174  {
175  goal[0] = g[0];
176  goal[1] = g[1];
177  ROS_DEBUG("[NavFn] Setting goal to %d,%d\n", goal[0], goal[1]);
178  }
179 
180  void
182  {
183  start[0] = g[0];
184  start[1] = g[1];
185  ROS_DEBUG("[NavFn] Setting start to %d,%d\n", start[0], start[1]);
186  }
187 
188  //
189  // Set/Reset map size
190  //
191 
192  void
193  NavFn::setNavArr(int xs, int ys)
194  {
195  ROS_DEBUG("[NavFn] Array is %d x %d\n", xs, ys);
196 
197  nx = xs;
198  ny = ys;
199  ns = nx*ny;
200 
201  if(costarr)
202  delete[] costarr;
203  if(potarr)
204  delete[] potarr;
205  if(pending)
206  delete[] pending;
207 
208  if(gradx)
209  delete[] gradx;
210  if(grady)
211  delete[] grady;
212 
213  costarr = new COSTTYPE[ns]; // cost array, 2d config space
214  memset(costarr, 0, ns*sizeof(COSTTYPE));
215  potarr = new float[ns]; // navigation potential array
216  pending = new bool[ns];
217  memset(pending, 0, ns*sizeof(bool));
218  gradx = new float[ns];
219  grady = new float[ns];
220  }
221 
222 
223  //
224  // set up cost array, usually from ROS
225  //
226 
227  void
228  NavFn::setCostmap(const COSTTYPE *cmap, bool isROS, bool allow_unknown)
229  {
230  COSTTYPE *cm = costarr;
231  if (isROS) // ROS-type cost array
232  {
233  for (int i=0; i<ny; i++)
234  {
235  int k=i*nx;
236  for (int j=0; j<nx; j++, k++, cmap++, cm++)
237  {
238  // This transforms the incoming cost values:
239  // COST_OBS -> COST_OBS (incoming "lethal obstacle")
240  // COST_OBS_ROS -> COST_OBS (incoming "inscribed inflated obstacle")
241  // values in range 0 to 252 -> values from COST_NEUTRAL to COST_OBS_ROS.
242  *cm = COST_OBS;
243  int v = *cmap;
244  if (v < COST_OBS_ROS)
245  {
247  if (v >= COST_OBS)
248  v = COST_OBS-1;
249  *cm = v;
250  }
251  else if(v == COST_UNKNOWN_ROS && allow_unknown)
252  {
253  v = COST_OBS-1;
254  *cm = v;
255  }
256  }
257  }
258  }
259 
260  else // not a ROS map, just a PGM
261  {
262  for (int i=0; i<ny; i++)
263  {
264  int k=i*nx;
265  for (int j=0; j<nx; j++, k++, cmap++, cm++)
266  {
267  *cm = COST_OBS;
268  if (i<7 || i > ny-8 || j<7 || j > nx-8)
269  continue; // don't do borders
270  int v = *cmap;
271  if (v < COST_OBS_ROS)
272  {
274  if (v >= COST_OBS)
275  v = COST_OBS-1;
276  *cm = v;
277  }
278  else if(v == COST_UNKNOWN_ROS)
279  {
280  v = COST_OBS-1;
281  *cm = v;
282  }
283  }
284  }
285 
286  }
287  }
288 
289  bool
291  {
292 #if 0
293  static char costmap_filename[1000];
294  static int file_number = 0;
295  snprintf( costmap_filename, 1000, "navfn-dijkstra-costmap-%04d", file_number++ );
296  savemap( costmap_filename );
297 #endif
298  setupNavFn(true);
299 
300  // calculate the nav fn and path
301  propNavFnDijkstra(std::max(nx*ny/20,nx+ny),atStart);
302 
303  // path
304  int len = calcPath(nx*ny/2);
305 
306  if (len > 0) // found plan
307  {
308  ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
309  return true;
310  }
311  else
312  {
313  ROS_DEBUG("[NavFn] No path found\n");
314  return false;
315  }
316 
317  }
318 
319 
320  //
321  // calculate navigation function, given a costmap, goal, and start
322  //
323 
324  bool
326  {
327  setupNavFn(true);
328 
329  // calculate the nav fn and path
330  propNavFnAstar(std::max(nx*ny/20,nx+ny));
331 
332  // path
333  int len = calcPath(nx*4);
334 
335  if (len > 0) // found plan
336  {
337  ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
338  return true;
339  }
340  else
341  {
342  ROS_DEBUG("[NavFn] No path found\n");
343  return false;
344  }
345  }
346 
347 
348  //
349  // returning values
350  //
351 
352  float *NavFn::getPathX() { return pathx; }
353  float *NavFn::getPathY() { return pathy; }
354  int NavFn::getPathLen() { return npath; }
355 
356 
357  //
358  // simple obstacle setup for tests
359  //
360 
361  void
363  {
364 #if 0
365  // set up a simple obstacle
366  ROS_INFO("[NavFn] Setting simple obstacle\n");
367  int xx = nx/3;
368  for (int i=ny/3; i<ny; i++)
369  costarr[i*nx + xx] = COST_OBS;
370  xx = 2*nx/3;
371  for (int i=ny/3; i<ny; i++)
372  costarr[i*nx + xx] = COST_OBS;
373 
374  xx = nx/4;
375  for (int i=20; i<ny-ny/3; i++)
376  costarr[i*nx + xx] = COST_OBS;
377  xx = nx/2;
378  for (int i=20; i<ny-ny/3; i++)
379  costarr[i*nx + xx] = COST_OBS;
380  xx = 3*nx/4;
381  for (int i=20; i<ny-ny/3; i++)
382  costarr[i*nx + xx] = COST_OBS;
383 #endif
384  }
385 
386 
387  // inserting onto the priority blocks
388 #define push_cur(n) { if (n>=0 && n<ns && !pending[n] && \
389  costarr[n]<COST_OBS && curPe<PRIORITYBUFSIZE) \
390  { curP[curPe++]=n; pending[n]=true; }}
391 #define push_next(n) { if (n>=0 && n<ns && !pending[n] && \
392  costarr[n]<COST_OBS && nextPe<PRIORITYBUFSIZE) \
393  { nextP[nextPe++]=n; pending[n]=true; }}
394 #define push_over(n) { if (n>=0 && n<ns && !pending[n] && \
395  costarr[n]<COST_OBS && overPe<PRIORITYBUFSIZE) \
396  { overP[overPe++]=n; pending[n]=true; }}
397 
398 
399  // Set up navigation potential arrays for new propagation
400 
401  void
402  NavFn::setupNavFn(bool keepit)
403  {
404  // reset values in propagation arrays
405  for (int i=0; i<ns; i++)
406  {
407  potarr[i] = POT_HIGH;
408  if (!keepit) costarr[i] = COST_NEUTRAL;
409  gradx[i] = grady[i] = 0.0;
410  }
411 
412  // outer bounds of cost array
413  COSTTYPE *pc;
414  pc = costarr;
415  for (int i=0; i<nx; i++)
416  *pc++ = COST_OBS;
417  pc = costarr + (ny-1)*nx;
418  for (int i=0; i<nx; i++)
419  *pc++ = COST_OBS;
420  pc = costarr;
421  for (int i=0; i<ny; i++, pc+=nx)
422  *pc = COST_OBS;
423  pc = costarr + nx - 1;
424  for (int i=0; i<ny; i++, pc+=nx)
425  *pc = COST_OBS;
426 
427  // priority buffers
428  curT = COST_OBS;
429  curP = pb1;
430  curPe = 0;
431  nextP = pb2;
432  nextPe = 0;
433  overP = pb3;
434  overPe = 0;
435  memset(pending, 0, ns*sizeof(bool));
436 
437  // set goal
438  int k = goal[0] + goal[1]*nx;
439  initCost(k,0);
440 
441  // find # of obstacle cells
442  pc = costarr;
443  int ntot = 0;
444  for (int i=0; i<ns; i++, pc++)
445  {
446  if (*pc >= COST_OBS)
447  ntot++; // number of cells that are obstacles
448  }
449  nobs = ntot;
450  }
451 
452 
453  // initialize a goal-type cost for starting propagation
454 
455  void
456  NavFn::initCost(int k, float v)
457  {
458  potarr[k] = v;
459  push_cur(k+1);
460  push_cur(k-1);
461  push_cur(k-nx);
462  push_cur(k+nx);
463  }
464 
465 
466  //
467  // Critical function: calculate updated potential value of a cell,
468  // given its neighbors' values
469  // Planar-wave update calculation from two lowest neighbors in a 4-grid
470  // Quadratic approximation to the interpolated value
471  // No checking of bounds here, this function should be fast
472  //
473 
474 #define INVSQRT2 0.707106781
475 
476  inline void
478  {
479  // get neighbors
480  float u,d,l,r;
481  l = potarr[n-1];
482  r = potarr[n+1];
483  u = potarr[n-nx];
484  d = potarr[n+nx];
485  // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",
486  // potarr[n], l, r, u, d);
487  // ROS_INFO("[Update] cost: %d\n", costarr[n]);
488 
489  // find lowest, and its lowest neighbor
490  float ta, tc;
491  if (l<r) tc=l; else tc=r;
492  if (u<d) ta=u; else ta=d;
493 
494  // do planar wave update
495  if (costarr[n] < COST_OBS) // don't propagate into obstacles
496  {
497  float hf = (float)costarr[n]; // traversability factor
498  float dc = tc-ta; // relative cost between ta,tc
499  if (dc < 0) // ta is lowest
500  {
501  dc = -dc;
502  ta = tc;
503  }
504 
505  // calculate new potential
506  float pot;
507  if (dc >= hf) // if too large, use ta-only update
508  pot = ta+hf;
509  else // two-neighbor interpolation update
510  {
511  // use quadratic approximation
512  // might speed this up through table lookup, but still have to
513  // do the divide
514  float d = dc/hf;
515  float v = -0.2301*d*d + 0.5307*d + 0.7040;
516  pot = ta + hf*v;
517  }
518 
519  // ROS_INFO("[Update] new pot: %d\n", costarr[n]);
520 
521  // now add affected neighbors to priority blocks
522  if (pot < potarr[n])
523  {
524  float le = INVSQRT2*(float)costarr[n-1];
525  float re = INVSQRT2*(float)costarr[n+1];
526  float ue = INVSQRT2*(float)costarr[n-nx];
527  float de = INVSQRT2*(float)costarr[n+nx];
528  potarr[n] = pot;
529  if (pot < curT) // low-cost buffer block
530  {
531  if (l > pot+le) push_next(n-1);
532  if (r > pot+re) push_next(n+1);
533  if (u > pot+ue) push_next(n-nx);
534  if (d > pot+de) push_next(n+nx);
535  }
536  else // overflow block
537  {
538  if (l > pot+le) push_over(n-1);
539  if (r > pot+re) push_over(n+1);
540  if (u > pot+ue) push_over(n-nx);
541  if (d > pot+de) push_over(n+nx);
542  }
543  }
544 
545  }
546 
547  }
548 
549 
550  //
551  // Use A* method for setting priorities
552  // Critical function: calculate updated potential value of a cell,
553  // given its neighbors' values
554  // Planar-wave update calculation from two lowest neighbors in a 4-grid
555  // Quadratic approximation to the interpolated value
556  // No checking of bounds here, this function should be fast
557  //
558 
559 #define INVSQRT2 0.707106781
560 
561  inline void
563  {
564  // get neighbors
565  float u,d,l,r;
566  l = potarr[n-1];
567  r = potarr[n+1];
568  u = potarr[n-nx];
569  d = potarr[n+nx];
570  //ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",
571  // potarr[n], l, r, u, d);
572  // ROS_INFO("[Update] cost of %d: %d\n", n, costarr[n]);
573 
574  // find lowest, and its lowest neighbor
575  float ta, tc;
576  if (l<r) tc=l; else tc=r;
577  if (u<d) ta=u; else ta=d;
578 
579  // do planar wave update
580  if (costarr[n] < COST_OBS) // don't propagate into obstacles
581  {
582  float hf = (float)costarr[n]; // traversability factor
583  float dc = tc-ta; // relative cost between ta,tc
584  if (dc < 0) // ta is lowest
585  {
586  dc = -dc;
587  ta = tc;
588  }
589 
590  // calculate new potential
591  float pot;
592  if (dc >= hf) // if too large, use ta-only update
593  pot = ta+hf;
594  else // two-neighbor interpolation update
595  {
596  // use quadratic approximation
597  // might speed this up through table lookup, but still have to
598  // do the divide
599  float d = dc/hf;
600  float v = -0.2301*d*d + 0.5307*d + 0.7040;
601  pot = ta + hf*v;
602  }
603 
604  //ROS_INFO("[Update] new pot: %d\n", costarr[n]);
605 
606  // now add affected neighbors to priority blocks
607  if (pot < potarr[n])
608  {
609  float le = INVSQRT2*(float)costarr[n-1];
610  float re = INVSQRT2*(float)costarr[n+1];
611  float ue = INVSQRT2*(float)costarr[n-nx];
612  float de = INVSQRT2*(float)costarr[n+nx];
613 
614  // calculate distance
615  int x = n%nx;
616  int y = n/nx;
617  float dist = hypot(x-start[0], y-start[1])*(float)COST_NEUTRAL;
618 
619  potarr[n] = pot;
620  pot += dist;
621  if (pot < curT) // low-cost buffer block
622  {
623  if (l > pot+le) push_next(n-1);
624  if (r > pot+re) push_next(n+1);
625  if (u > pot+ue) push_next(n-nx);
626  if (d > pot+de) push_next(n+nx);
627  }
628  else
629  {
630  if (l > pot+le) push_over(n-1);
631  if (r > pot+re) push_over(n+1);
632  if (u > pot+ue) push_over(n-nx);
633  if (d > pot+de) push_over(n+nx);
634  }
635  }
636 
637  }
638 
639  }
640 
641 
642 
643  //
644  // main propagation function
645  // Dijkstra method, breadth-first
646  // runs for a specified number of cycles,
647  // or until it runs out of cells to update,
648  // or until the Start cell is found (atStart = true)
649  //
650 
651  bool
652  NavFn::propNavFnDijkstra(int cycles, bool atStart)
653  {
654  int nwv = 0; // max priority block size
655  int nc = 0; // number of cells put into priority blocks
656  int cycle = 0; // which cycle we're on
657 
658  // set up start cell
659  int startCell = start[1]*nx + start[0];
660 
661  for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
662  {
663  //
664  if (curPe == 0 && nextPe == 0) // priority blocks empty
665  break;
666 
667  // stats
668  nc += curPe;
669  if (curPe > nwv)
670  nwv = curPe;
671 
672  // reset pending flags on current priority buffer
673  int *pb = curP;
674  int i = curPe;
675  while (i-- > 0)
676  pending[*(pb++)] = false;
677 
678  // process current priority buffer
679  pb = curP;
680  i = curPe;
681  while (i-- > 0)
682  updateCell(*pb++);
683 
684  if (displayInt > 0 && (cycle % displayInt) == 0)
685  displayFn(this);
686 
687  // swap priority blocks curP <=> nextP
688  curPe = nextPe;
689  nextPe = 0;
690  pb = curP; // swap buffers
691  curP = nextP;
692  nextP = pb;
693 
694  // see if we're done with this priority level
695  if (curPe == 0)
696  {
697  curT += priInc; // increment priority threshold
698  curPe = overPe; // set current to overflow block
699  overPe = 0;
700  pb = curP; // swap buffers
701  curP = overP;
702  overP = pb;
703  }
704 
705  // check if we've hit the Start cell
706  if (atStart)
707  if (potarr[startCell] < POT_HIGH)
708  break;
709  }
710 
711  ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n",
712  cycle,nc,(int)((nc*100.0)/(ns-nobs)),nwv);
713 
714  if (cycle < cycles) return true; // finished up here
715  else return false;
716  }
717 
718 
719  //
720  // main propagation function
721  // A* method, best-first
722  // uses Euclidean distance heuristic
723  // runs for a specified number of cycles,
724  // or until it runs out of cells to update,
725  // or until the Start cell is found (atStart = true)
726  //
727 
728  bool
730  {
731  int nwv = 0; // max priority block size
732  int nc = 0; // number of cells put into priority blocks
733  int cycle = 0; // which cycle we're on
734 
735  // set initial threshold, based on distance
736  float dist = hypot(goal[0]-start[0], goal[1]-start[1])*(float)COST_NEUTRAL;
737  curT = dist + curT;
738 
739  // set up start cell
740  int startCell = start[1]*nx + start[0];
741 
742  // do main cycle
743  for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
744  {
745  //
746  if (curPe == 0 && nextPe == 0) // priority blocks empty
747  break;
748 
749  // stats
750  nc += curPe;
751  if (curPe > nwv)
752  nwv = curPe;
753 
754  // reset pending flags on current priority buffer
755  int *pb = curP;
756  int i = curPe;
757  while (i-- > 0)
758  pending[*(pb++)] = false;
759 
760  // process current priority buffer
761  pb = curP;
762  i = curPe;
763  while (i-- > 0)
764  updateCellAstar(*pb++);
765 
766  if (displayInt > 0 && (cycle % displayInt) == 0)
767  displayFn(this);
768 
769  // swap priority blocks curP <=> nextP
770  curPe = nextPe;
771  nextPe = 0;
772  pb = curP; // swap buffers
773  curP = nextP;
774  nextP = pb;
775 
776  // see if we're done with this priority level
777  if (curPe == 0)
778  {
779  curT += priInc; // increment priority threshold
780  curPe = overPe; // set current to overflow block
781  overPe = 0;
782  pb = curP; // swap buffers
783  curP = overP;
784  overP = pb;
785  }
786 
787  // check if we've hit the Start cell
788  if (potarr[startCell] < POT_HIGH)
789  break;
790 
791  }
792 
793  last_path_cost_ = potarr[startCell];
794 
795  ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n",
796  cycle,nc,(int)((nc*100.0)/(ns-nobs)),nwv);
797 
798 
799  if (potarr[startCell] < POT_HIGH) return true; // finished up here
800  else return false;
801  }
802 
803 
805  {
806  return last_path_cost_;
807  }
808 
809 
810  //
811  // Path construction
812  // Find gradient at array points, interpolate path
813  // Use step size of pathStep, usually 0.5 pixel
814  //
815  // Some sanity checks:
816  // 1. Stuck at same index position
817  // 2. Doesn't get near goal
818  // 3. Surrounded by high potentials
819  //
820 
821  int
822  NavFn::calcPath(int n, int *st)
823  {
824  // test write
825  //savemap("test");
826 
827  // check path arrays
828  if (npathbuf < n)
829  {
830  if (pathx) delete [] pathx;
831  if (pathy) delete [] pathy;
832  pathx = new float[n];
833  pathy = new float[n];
834  npathbuf = n;
835  }
836 
837  // set up start position at cell
838  // st is always upper left corner for 4-point bilinear interpolation
839  if (st == NULL) st = start;
840  int stc = st[1]*nx + st[0];
841 
842  // set up offset
843  float dx=0;
844  float dy=0;
845  npath = 0;
846 
847  // go for <n> cycles at most
848  for (int i=0; i<n; i++)
849  {
850  // check if near goal
851  int nearest_point=std::max(0,std::min(nx*ny-1,stc+(int)round(dx)+(int)(nx*round(dy))));
852  if (potarr[nearest_point] < COST_NEUTRAL)
853  {
854  pathx[npath] = (float)goal[0];
855  pathy[npath] = (float)goal[1];
856  return ++npath; // done!
857  }
858 
859  if (stc < nx || stc > ns-nx) // would be out of bounds
860  {
861  ROS_DEBUG("[PathCalc] Out of bounds");
862  return 0;
863  }
864 
865  // add to path
866  pathx[npath] = stc%nx + dx;
867  pathy[npath] = stc/nx + dy;
868  npath++;
869 
870  bool oscillation_detected = false;
871  if( npath > 2 &&
872  pathx[npath-1] == pathx[npath-3] &&
873  pathy[npath-1] == pathy[npath-3] )
874  {
875  ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
876  oscillation_detected = true;
877  }
878 
879  int stcnx = stc+nx;
880  int stcpx = stc-nx;
881 
882  // check for potentials at eight positions near cell
883  if (potarr[stc] >= POT_HIGH ||
884  potarr[stc+1] >= POT_HIGH ||
885  potarr[stc-1] >= POT_HIGH ||
886  potarr[stcnx] >= POT_HIGH ||
887  potarr[stcnx+1] >= POT_HIGH ||
888  potarr[stcnx-1] >= POT_HIGH ||
889  potarr[stcpx] >= POT_HIGH ||
890  potarr[stcpx+1] >= POT_HIGH ||
891  potarr[stcpx-1] >= POT_HIGH ||
892  oscillation_detected)
893  {
894  ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath);
895  // check eight neighbors to find the lowest
896  int minc = stc;
897  int minp = potarr[stc];
898  int st = stcpx - 1;
899  if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
900  st++;
901  if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
902  st++;
903  if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
904  st = stc-1;
905  if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
906  st = stc+1;
907  if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
908  st = stcnx-1;
909  if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
910  st++;
911  if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
912  st++;
913  if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
914  stc = minc;
915  dx = 0;
916  dy = 0;
917 
918  ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f",
919  potarr[stc], pathx[npath-1], pathy[npath-1]);
920 
921  if (potarr[stc] >= POT_HIGH)
922  {
923  ROS_DEBUG("[PathCalc] No path found, high potential");
924  //savemap("navfn_highpot");
925  return 0;
926  }
927  }
928 
929  // have a good gradient here
930  else
931  {
932 
933  // get grad at four positions near cell
934  gradCell(stc);
935  gradCell(stc+1);
936  gradCell(stcnx);
937  gradCell(stcnx+1);
938 
939 
940  // get interpolated gradient
941  float x1 = (1.0-dx)*gradx[stc] + dx*gradx[stc+1];
942  float x2 = (1.0-dx)*gradx[stcnx] + dx*gradx[stcnx+1];
943  float x = (1.0-dy)*x1 + dy*x2; // interpolated x
944  float y1 = (1.0-dx)*grady[stc] + dx*grady[stc+1];
945  float y2 = (1.0-dx)*grady[stcnx] + dx*grady[stcnx+1];
946  float y = (1.0-dy)*y1 + dy*y2; // interpolated y
947 
948  // show gradients
949  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",
950  gradx[stc], grady[stc], gradx[stc+1], grady[stc+1],
951  gradx[stcnx], grady[stcnx], gradx[stcnx+1], grady[stcnx+1],
952  x, y);
953 
954  // check for zero gradient, failed
955  if (x == 0.0 && y == 0.0)
956  {
957  ROS_DEBUG("[PathCalc] Zero gradient");
958  return 0;
959  }
960 
961  // move in the right direction
962  float ss = pathStep/hypot(x, y);
963  dx += x*ss;
964  dy += y*ss;
965 
966  // check for overflow
967  if (dx > 1.0) { stc++; dx -= 1.0; }
968  if (dx < -1.0) { stc--; dx += 1.0; }
969  if (dy > 1.0) { stc+=nx; dy -= 1.0; }
970  if (dy < -1.0) { stc-=nx; dy += 1.0; }
971 
972  }
973 
974  // ROS_INFO("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n",
975  // potarr[stc], x, y, pathx[npath-1], pathy[npath-1]);
976  }
977 
978  // return npath; // out of cycles, return failure
979  ROS_DEBUG("[PathCalc] No path found, path too long");
980  //savemap("navfn_pathlong");
981  return 0; // out of cycles, return failure
982  }
983 
984 
985  //
986  // gradient calculations
987  //
988 
989  // calculate gradient at a cell
990  // positive value are to the right and down
991  float
993  {
994  if (gradx[n]+grady[n] > 0.0) // check this cell
995  return 1.0;
996 
997  if (n < nx || n > ns-nx) // would be out of bounds
998  return 0.0;
999 
1000  float cv = potarr[n];
1001  float dx = 0.0;
1002  float dy = 0.0;
1003 
1004  // check for in an obstacle
1005  if (cv >= POT_HIGH)
1006  {
1007  if (potarr[n-1] < POT_HIGH)
1008  dx = -COST_OBS;
1009  else if (potarr[n+1] < POT_HIGH)
1010  dx = COST_OBS;
1011 
1012  if (potarr[n-nx] < POT_HIGH)
1013  dy = -COST_OBS;
1014  else if (potarr[n+nx] < POT_HIGH)
1015  dy = COST_OBS;
1016  }
1017 
1018  else // not in an obstacle
1019  {
1020  // dx calc, average to sides
1021  if (potarr[n-1] < POT_HIGH)
1022  dx += potarr[n-1]- cv;
1023  if (potarr[n+1] < POT_HIGH)
1024  dx += cv - potarr[n+1];
1025 
1026  // dy calc, average to sides
1027  if (potarr[n-nx] < POT_HIGH)
1028  dy += potarr[n-nx]- cv;
1029  if (potarr[n+nx] < POT_HIGH)
1030  dy += cv - potarr[n+nx];
1031  }
1032 
1033  // normalize
1034  float norm = hypot(dx, dy);
1035  if (norm > 0)
1036  {
1037  norm = 1.0/norm;
1038  gradx[n] = norm*dx;
1039  grady[n] = norm*dy;
1040  }
1041  return norm;
1042  }
1043 
1044 
1045  //
1046  // display function setup
1047  // <n> is the number of cycles to wait before displaying,
1048  // use 0 to turn it off
1049 
1050  void
1051  NavFn::display(void fn(NavFn *nav), int n)
1052  {
1053  displayFn = fn;
1054  displayInt = n;
1055  }
1056 
1057 
1058  //
1059  // debug writes
1060  // saves costmap and start/goal
1061  //
1062 
1063  void
1064  NavFn::savemap(const char *fname)
1065  {
1066  char fn[4096];
1067 
1068  ROS_DEBUG("[NavFn] Saving costmap and start/goal points");
1069  // write start and goal points
1070  sprintf(fn,"%s.txt",fname);
1071  FILE *fp = fopen(fn,"w");
1072  if (!fp)
1073  {
1074  ROS_WARN("Can't open file %s", fn);
1075  return;
1076  }
1077  fprintf(fp,"Goal: %d %d\nStart: %d %d\n",goal[0],goal[1],start[0],start[1]);
1078  fclose(fp);
1079 
1080  // write cost array
1081  if (!costarr) return;
1082  sprintf(fn,"%s.pgm",fname);
1083  fp = fopen(fn,"wb");
1084  if (!fp)
1085  {
1086  ROS_WARN("Can't open file %s", fn);
1087  return;
1088  }
1089  fprintf(fp,"P5\n%d\n%d\n%d\n", nx, ny, 0xff);
1090  fwrite(costarr,1,nx*ny,fp);
1091  fclose(fp);
1092  }
1093 };
int * pb2
Definition: navfn.h:176
d
void updateCell(int n)
Updates the cell at index n.
Definition: navfn.cpp:477
Definition: navfn.h:81
void setupNavFn(bool keepit=false)
Definition: navfn.cpp:402
NavFn(int nx, int ny)
Constructs the planner.
Definition: navfn.cpp:110
float * getPathY()
Accessor for the y-coordinates of a path.
Definition: navfn.cpp:353
int npath
Definition: navfn.h:242
int nextPe
Definition: navfn.h:178
int * curP
Definition: navfn.h:177
int overPe
Definition: navfn.h:178
float * pathx
Definition: navfn.h:241
float curT
Definition: navfn.h:181
int create_nav_plan_astar(const COSTTYPE *costmap, int nx, int ny, int *goal, int *start, float *plan, int nplan)
bool * pending
Definition: navfn.h:172
void(* displayFn)(NavFn *nav)
Definition: navfn.h:261
void updateCellAstar(int n)
Updates the cell at index n using the A* heuristic.
Definition: navfn.cpp:562
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void setStart(int *start)
Sets the start position for the planner. Note: the navigation cost field computed gives the cost to g...
Definition: navfn.cpp:181
void setCostmap(const COSTTYPE *cmap, bool isROS=true, bool allow_unknown=true)
Set up the cost array for the planner, usually from ROS.
Definition: navfn.cpp:228
bool calcNavFnDijkstra(bool atStart=false)
Caclulates the full navigation function using Dijkstra.
Definition: navfn.cpp:290
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
int nx
Definition: navfn.h:124
int ns
Definition: navfn.h:124
float * potarr
Definition: navfn.h:171
COSTTYPE * costarr
Definition: navfn.h:170
float last_path_cost_
Definition: navfn.h:245
float getLastPathCost()
Gets the cost of the path found the last time a navigation function was computed. ...
Definition: navfn.cpp:804
float * gradx
Definition: navfn.h:240
float * grady
Definition: navfn.h:240
void setGoal(int *goal)
Sets the goal position for the planner. Note: the navigation cost field computed gives the cost to ge...
Definition: navfn.cpp:173
float gradCell(int n)
Definition: navfn.cpp:992
void setObs()
Definition: navfn.cpp:362
int npathbuf
Definition: navfn.h:243
#define ROS_INFO(...)
Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.
Definition: navfn.h:106
float pathStep
Definition: navfn.h:256
int goal[2]
Definition: navfn.h:197
int start[2]
Definition: navfn.h:198
TFSIMD_FORCE_INLINE const tfScalar & x() const
int calcPath(int n, int *st=NULL)
Calculates the path for at mose <n> cycles.
Definition: navfn.cpp:822
int nobs
Definition: navfn.h:173
void initCost(int k, float v)
Initialize cell k with cost v for propagation.
Definition: navfn.cpp:456
void savemap(const char *fname)
Definition: navfn.cpp:1064
float * pathy
Definition: navfn.h:241
bool propNavFnDijkstra(int cycles, bool atStart=false)
Run propagation for <cycles> iterations, or until start is reached using breadth-first Dijkstra metho...
Definition: navfn.cpp:652
void display(void fn(NavFn *nav), int n=100)
Definition: navfn.cpp:1051
float priInc
Definition: navfn.h:182
int curPe
Definition: navfn.h:178
int getPathLen()
Accessor for the length of a path.
Definition: navfn.cpp:354
int * pb3
Definition: navfn.h:176
void setNavArr(int nx, int ny)
Sets or resets the size of the map.
Definition: navfn.cpp:193
int * overP
Definition: navfn.h:177
int displayInt
Definition: navfn.h:260
int * nextP
Definition: navfn.h:177
int * pb1
Definition: navfn.h:176
float * getPathX()
Accessor for the x-coordinates of a path.
Definition: navfn.cpp:352
bool calcNavFnAstar()
Calculates a plan using the A* heuristic, returns true if one is found.
Definition: navfn.cpp:325
bool propNavFnAstar(int cycles)
Run propagation for <cycles> iterations, or until start is reached using the best-first A* method wit...
Definition: navfn.cpp:729
int ny
Definition: navfn.h:124
#define ROS_DEBUG(...)


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:06:04