Program Listing for File navfn.hpp
↰ Return to documentation for file (include/nav2_navfn_planner/navfn.hpp
)
// 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_