navfn.h
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 //
00036 // Navigation function computation
00037 // Uses Dijkstra's method
00038 // Modified for Euclidean-distance computation
00039 //
00040 
00041 #ifndef _NAVFN_H
00042 #define _NAVFN_H
00043 
00044 #include <math.h>
00045 #include <stdint.h>
00046 #include <string.h>
00047 #include <stdio.h>
00048 
00049 // cost defs
00050 #define COST_UNKNOWN_ROS 255            // 255 is unknown cost
00051 #define COST_OBS 254            // 254 for forbidden regions
00052 #define COST_OBS_ROS 253        // ROS values of 253 are obstacles
00053 
00054 // navfn cost values are set to
00055 // COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
00056 // Incoming costmap cost values are in the range 0 to 252.
00057 // With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to
00058 // ensure the input values are spread evenly over the output range, 50
00059 // to 253.  If COST_FACTOR is higher, cost values will have a plateau
00060 // around obstacles and the planner will then treat (for example) the
00061 // whole width of a narrow hallway as equally undesirable and thus
00062 // will not plan paths down the center.
00063 
00064 #define COST_NEUTRAL 50         // Set this to "open space" value
00065 #define COST_FACTOR 0.8         // Used for translating costs in NavFn::setCostmap()
00066 
00067 // Define the cost type in the case that it is not set. However, this allows
00068 // clients to modify it without changing the file. Arguably, it is better to require it to
00069 // be defined by a user explicitly
00070 #ifndef COSTTYPE
00071 #define COSTTYPE unsigned char  // Whatever is used...
00072 #endif
00073 
00074 // potential defs
00075 #define POT_HIGH 1.0e10         // unassigned cell potential
00076 
00077 // priority buffers
00078 #define PRIORITYBUFSIZE 10000
00079 
00080 
00081 namespace navfn {
00096   int create_nav_plan_astar(const COSTTYPE *costmap, int nx, int ny,
00097       int* goal, int* start,
00098       float *plan, int nplan);
00099 
00100 
00101 
00106   class NavFn
00107   {
00108     public:
00114       NavFn(int nx, int ny);    // size of map
00115 
00116       ~NavFn();
00117 
00123       void setNavArr(int nx, int ny); 
00124       int nx, ny, ns;           
00132       void setCostmap(const COSTTYPE *cmap, bool isROS=true, bool allow_unknown = true); 
00138       bool calcNavFnAstar();    
00143       bool calcNavFnDijkstra(bool atStart = false);     
00149       float *getPathX();                
00155       float *getPathY();                
00161       int   getPathLen();               
00167       float getLastPathCost();      
00170       COSTTYPE *costarr;                
00171       float   *potarr;          
00172       bool    *pending;         
00173       int nobs;                 
00176       int *pb1, *pb2, *pb3;             
00177       int *curP, *nextP, *overP;        
00178       int curPe, nextPe, overPe; 
00181       float curT;                       
00182       float priInc;                     
00189       void setGoal(int *goal);  
00190 
00195       void setStart(int *start);        
00196 
00197       int goal[2];
00198       int start[2];
00204       void initCost(int k, float v); 
00207       void setObs();
00208 
00215       void updateCell(int n);   
00221       void updateCellAstar(int n);      
00223       void setupNavFn(bool keepit = false); 
00231       bool propNavFnDijkstra(int cycles, bool atStart = false); 
00237       bool propNavFnAstar(int cycles); 
00240       float *gradx, *grady;             
00241       float *pathx, *pathy;             
00242       int npath;                        
00243       int npathbuf;                     
00245       float last_path_cost_; 
00253       int calcPath(int n, int *st = NULL); 
00255       float gradCell(int n);    
00256       float pathStep;           
00259       void display(void fn(NavFn *nav), int n = 100); 
00260       int displayInt;           
00261       void (*displayFn)(NavFn *nav); 
00264       void savemap(const char *fname); 
00266   };
00267 };
00268 
00269 
00270 #endif  // NAVFN


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