navfn.h
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 //
36 // Navigation function computation
37 // Uses Dijkstra's method
38 // Modified for Euclidean-distance computation
39 //
40 
41 #ifndef _NAVFN_H
42 #define _NAVFN_H
43 
44 #include <math.h>
45 #include <stdint.h>
46 #include <string.h>
47 #include <stdio.h>
48 
49 // cost defs
50 #define COST_UNKNOWN_ROS 255 // 255 is unknown cost
51 #define COST_OBS 254 // 254 for forbidden regions
52 #define COST_OBS_ROS 253 // ROS values of 253 are obstacles
53 
54 // navfn cost values are set to
55 // COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
56 // Incoming costmap cost values are in the range 0 to 252.
57 // With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to
58 // ensure the input values are spread evenly over the output range, 50
59 // to 253. If COST_FACTOR is higher, cost values will have a plateau
60 // around obstacles and the planner will then treat (for example) the
61 // whole width of a narrow hallway as equally undesirable and thus
62 // will not plan paths down the center.
63 
64 #define COST_NEUTRAL 50 // Set this to "open space" value
65 #define COST_FACTOR 0.8 // Used for translating costs in NavFn::setCostmap()
66 
67 // Define the cost type in the case that it is not set. However, this allows
68 // clients to modify it without changing the file. Arguably, it is better to require it to
69 // be defined by a user explicitly
70 #ifndef COSTTYPE
71 #define COSTTYPE unsigned char // Whatever is used...
72 #endif
73 
74 // potential defs
75 #define POT_HIGH 1.0e10 // unassigned cell potential
76 
77 // priority buffers
78 #define PRIORITYBUFSIZE 10000
79 
80 
81 namespace navfn {
96  int create_nav_plan_astar(const COSTTYPE *costmap, int nx, int ny,
97  int* goal, int* start,
98  float *plan, int nplan);
99 
100 
101 
106  class NavFn
107  {
108  public:
114  NavFn(int nx, int ny); // size of map
115 
116  ~NavFn();
117 
123  void setNavArr(int nx, int ny);
124  int nx, ny, ns;
132  void setCostmap(const COSTTYPE *cmap, bool isROS=true, bool allow_unknown = true);
138  bool calcNavFnAstar();
143  bool calcNavFnDijkstra(bool atStart = false);
149  float *getPathX();
155  float *getPathY();
161  int getPathLen();
167  float getLastPathCost();
171  float *potarr;
172  bool *pending;
173  int nobs;
176  int *pb1, *pb2, *pb3;
177  int *curP, *nextP, *overP;
181  float curT;
182  float priInc;
189  void setGoal(int *goal);
190 
195  void setStart(int *start);
196 
197  int goal[2];
198  int start[2];
204  void initCost(int k, float v);
212  void updateCell(int n);
218  void updateCellAstar(int n);
220  void setupNavFn(bool keepit = false);
228  bool propNavFnDijkstra(int cycles, bool atStart = false);
234  bool propNavFnAstar(int cycles);
237  float *gradx, *grady;
238  float *pathx, *pathy;
239  int npath;
240  int npathbuf;
250  int calcPath(int n, int *st = NULL);
252  float gradCell(int n);
253  float pathStep;
256  void display(void fn(NavFn *nav), int n = 100);
258  void (*displayFn)(NavFn *nav);
261  void savemap(const char *fname);
263  };
264 };
265 
266 
267 #endif // NAVFN
int * pb2
Definition: navfn.h:176
void updateCell(int n)
Updates the cell at index n.
Definition: navfn.cpp:440
Definition: navfn.h:81
void setupNavFn(bool keepit=false)
Definition: navfn.cpp:365
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:347
int npath
Definition: navfn.h:239
int nextPe
Definition: navfn.h:178
int * curP
Definition: navfn.h:177
int overPe
Definition: navfn.h:178
float * pathx
Definition: navfn.h:238
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:258
void updateCellAstar(int n)
Updates the cell at index n using the A* heuristic.
Definition: navfn.cpp:525
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
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:242
float getLastPathCost()
Gets the cost of the path found the last time a navigation function was computed. ...
Definition: navfn.cpp:767
float * gradx
Definition: navfn.h:237
float * grady
Definition: navfn.h:237
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:955
int npathbuf
Definition: navfn.h:240
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:253
int calcPath(int n, int *st=NULL)
Calculates the path for at mose <n> cycles.
Definition: navfn.cpp:785
int nobs
Definition: navfn.h:173
void initCost(int k, float v)
Initialize cell k with cost v for propagation.
Definition: navfn.cpp:419
void savemap(const char *fname)
Definition: navfn.cpp:1027
float * pathy
Definition: navfn.h:238
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:615
void display(void fn(NavFn *nav), int n=100)
Definition: navfn.cpp:1014
float priInc
Definition: navfn.h:182
int curPe
Definition: navfn.h:178
int getPathLen()
Accessor for the length of a path.
Definition: navfn.cpp:348
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:257
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:346
bool calcNavFnAstar()
Calculates a plan using the A* heuristic, returns true if one is found.
Definition: navfn.cpp:319
bool propNavFnAstar(int cycles)
Run propagation for <cycles> iterations, or until start is reached using the best-first A* method wit...
Definition: navfn.cpp:692
int ny
Definition: navfn.h:124


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:39