navfn.hpp

// Copyright (c) 2008, Willow Garage, Inc.
//
// 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 copyright holder 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 HOLDER 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>
#include <functional>
 
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(std::function<bool()> cancelChecker);
 
  bool calcNavFnDijkstra(std::function<bool()> cancelChecker, 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;  
  static constexpr int terminal_checking_interval = 5000;
 
  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, std::function<bool()> cancelChecker, bool atStart = false);
 
  bool propNavFnAstar(int cycles, std::function<bool()> cancelChecker);
 
  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_