.. _program_listing_file__tmp_ws_src_navigation2_nav2_navfn_planner_include_nav2_navfn_planner_navfn.hpp:

Program Listing for File navfn.hpp
==================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_navigation2_nav2_navfn_planner_include_nav2_navfn_planner_navfn.hpp>` (``/tmp/ws/src/navigation2/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // Copyright (c) 2008, Willow Garage, Inc.
   // All rights reserved.
   //
   // Software License Agreement (BSD License 2.0)
   //
   // Redistribution and use in source and binary forms, with or without
   // modification, are permitted provided that the following conditions
   // are met:
   //
   //  * Redistributions of source code must retain the above copyright
   //    notice, this list of conditions and the following disclaimer.
   //  * Redistributions in binary form must reproduce the above
   //    copyright notice, this list of conditions and the following
   //    disclaimer in the documentation and/or other materials provided
   //    with the distribution.
   //  * Neither the name of the Willow Garage nor the names of its
   //    contributors may be used to endorse or promote products derived
   //    from this software without specific prior written permission.
   //
   // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
   // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
   // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
   // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
   // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
   // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
   // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   // POSSIBILITY OF SUCH DAMAGE.
   
   //
   // Navigation function computation
   // Uses Dijkstra's method
   // Modified for Euclidean-distance computation
   //
   
   #ifndef NAV2_NAVFN_PLANNER__NAVFN_HPP_
   #define NAV2_NAVFN_PLANNER__NAVFN_HPP_
   
   #include <math.h>
   #include <stdint.h>
   #include <string.h>
   #include <stdio.h>
   
   namespace nav2_navfn_planner
   {
   
   // cost defs
   #define COST_UNKNOWN_ROS 255  // 255 is unknown cost
   #define COST_OBS 254  // 254 for forbidden regions
   #define COST_OBS_ROS 253  // ROS values of 253 are obstacles
   
   // navfn cost values are set to
   // COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
   // Incoming costmap cost values are in the range 0 to 252.
   // With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to
   // ensure the input values are spread evenly over the output range, 50
   // to 253.  If COST_FACTOR is higher, cost values will have a plateau
   // around obstacles and the planner will then treat (for example) the
   // whole width of a narrow hallway as equally undesirable and thus
   // will not plan paths down the center.
   
   #define COST_NEUTRAL 50  // Set this to "open space" value
   #define COST_FACTOR 0.8  // Used for translating costs in NavFn::setCostmap()
   
   // Define the cost type in the case that it is not set. However, this allows
   // clients to modify it without changing the file. Arguably, it is better to require it to
   // be defined by a user explicitly
   #ifndef COSTTYPE
   #define COSTTYPE unsigned char  // Whatever is used...
   #endif
   
   // potential defs
   #define POT_HIGH 1.0e10  // unassigned cell potential
   
   // priority buffers
   #define PRIORITYBUFSIZE 10000
   
   int create_nav_plan_astar(
     const COSTTYPE * costmap, int nx, int ny,
     int * goal, int * start,
     float * plan, int nplan);
   
   class NavFn
   {
   public:
     NavFn(int nx, int ny);
   
     ~NavFn();
   
     void setNavArr(int nx, int ny);
     int nx, ny, ns;  
     void setCostmap(const COSTTYPE * cmap, bool isROS = true, bool allow_unknown = true);
   
     bool calcNavFnAstar();
   
     bool calcNavFnDijkstra(bool atStart = false);
   
     float * getPathX();
   
     float * getPathY();
   
     int getPathLen();
   
     float getLastPathCost();
   
     COSTTYPE * costarr;  
     float * potarr;  
     bool * pending;  
     int nobs;  
     int * pb1, * pb2, * pb3;  
     int * curP, * nextP, * overP;  
     int curPe, nextPe, overPe;  
     float curT;  
     float priInc;  
     void setGoal(int * goal);
   
     void setStart(int * start);
   
     int goal[2];
     int start[2];
     void initCost(int k, float v);
   
     void updateCell(int n);
   
     void updateCellAstar(int n);
   
     void setupNavFn(bool keepit = false);
   
     bool propNavFnDijkstra(int cycles, bool atStart = false);
   
     bool propNavFnAstar(int cycles);  
     float * gradx, * grady;  
     float * pathx, * pathy;  
     int npath;  
     int npathbuf;  
     float last_path_cost_;  
     int calcPath(int n, int * st = NULL);
   
     float gradCell(int n);  
     float pathStep;  
     // void display(void fn(NavFn * nav), int n = 100);
     // int displayInt;  /**< save second argument of display() above */
     // void (* displayFn)(NavFn * nav);  /**< display function itself */
   
     // void savemap(const char * fname);
   };
   
   }  // namespace nav2_navfn_planner
   
   #endif  // NAV2_NAVFN_PLANNER__NAVFN_HPP_