Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down. More...

#include <nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp>

Public Member Functions
	NavFn (int nx, int ny)
	Constructs the planner. More...

void	setNavArr (int nx, int ny)
	Sets or resets the size of the map. More...

void	setCostmap (const COSTTYPE *cmap, bool isROS=true, bool allow_unknown=true)
	Set up the cost array for the planner, usually from ROS. More...

bool	calcNavFnAstar (std::function< bool()> cancelChecker)
	Calculates a plan using the A* heuristic, returns true if one is found. More...

bool	calcNavFnDijkstra (std::function< bool()> cancelChecker, bool atStart=false)
	Calculates the full navigation function using Dijkstra. More...

float *	getPathX ()
	Accessor for the x-coordinates of a path. More...

float *	getPathY ()
	Accessor for the y-coordinates of a path. More...

int	getPathLen ()
	Accessor for the length of a path. More...

float	getLastPathCost ()
	Gets the cost of the path found the last time a navigation function was computed. More...

void	setGoal (int *goal)
	Sets the goal position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start. More...

void	setStart (int *start)
	Sets the start position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start. More...

void	initCost (int k, float v)
	Initialize cell k with cost v for propagation. More...

void	updateCell (int n)
	Updates the cell at index n. More...

void	updateCellAstar (int n)
	Updates the cell at index n using the A* heuristic. More...

void	setupNavFn (bool keepit=false)
	Set up navigation potential arrays for new propagation. More...

bool	propNavFnDijkstra (int cycles, std::function< bool()> cancelChecker, bool atStart=false)
	Run propagation for <cycles> iterations, or until start is reached using breadth-first Dijkstra method. More...

bool	propNavFnAstar (int cycles, std::function< bool()> cancelChecker)
	Run propagation for <cycles> iterations, or until start is reached using the best-first A* method with Euclidean distance heuristic. More...

int	calcPath (int n, int *st=NULL)
	Calculates the path for at most <n> cycles. More...

float	gradCell (int n)
	Calculate gradient at a cell. More...

Public Attributes
int	nx

int	ny

int	ns

COSTTYPE *	costarr

float *	potarr

bool *	pending

int	nobs

int *	pb1

int *	pb2

int *	pb3

int *	curP

int *	nextP

int *	overP

int	curPe

int	nextPe

int	overPe

float	curT

float	priInc

int	goal [2]

int	start [2]

float *	gradx

float *	grady

float *	pathx

float *	pathy

int	npath

int	npathbuf

float	last_path_cost_

float	pathStep

Static Public Attributes
static constexpr int	terminal_checking_interval = 5000

Detailed Description

Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.

Definition at line 104 of file navfn.hpp.

Constructor & Destructor Documentation

◆ NavFn()

nav2_navfn_planner::NavFn::NavFn	(	int	nx,
		int	ny
	)

Constructs the planner.

Parameters

nx	The x size of the map
ny	The y size of the map

Definition at line 112 of file navfn.cpp.

References costarr, gradx, grady, npath, npathbuf, pathStep, pathy, pb1, pb3, pending, potarr, priInc, and setNavArr().

Here is the call graph for this function:

Member Function Documentation

◆ calcNavFnAstar()

bool nav2_navfn_planner::NavFn::calcNavFnAstar ( std::function< bool()> cancelChecker )

Calculates a plan using the A* heuristic, returns true if one is found.

Parameters

cancelChecker Function to check if the task has been canceled

Returns: True if a plan is found, false otherwise

Definition at line 311 of file navfn.cpp.

References propNavFnAstar(), and setupNavFn().

Here is the call graph for this function:

◆ calcNavFnDijkstra()

bool nav2_navfn_planner::NavFn::calcNavFnDijkstra	(	std::function< bool()>	cancelChecker,
		bool	atStart = `false`
	)

Calculates the full navigation function using Dijkstra.

Parameters

cancelChecker Function to check if the task has been canceled

Definition at line 297 of file navfn.cpp.

References propNavFnDijkstra(), and setupNavFn().

Here is the call graph for this function:

◆ calcPath()

int nav2_navfn_planner::NavFn::calcPath	(	int	n,
		int *	st = `NULL`
	)

Calculates the path for at most <n> cycles.

Parameters

n	The maximum number of cycles to run for

Returns: The length of the path found, 0 if none

Definition at line 763 of file navfn.cpp.

References gradCell(), gradx, grady, npath, npathbuf, ns, pathStep, pathy, and potarr.

Here is the call graph for this function:

◆ getLastPathCost()

float nav2_navfn_planner::NavFn::getLastPathCost ( )

Gets the cost of the path found the last time a navigation function was computed.

Returns: The cost of the last path found

Definition at line 745 of file navfn.cpp.

References last_path_cost_.

◆ getPathLen()

int nav2_navfn_planner::NavFn::getPathLen ( )

Accessor for the length of a path.

Returns: The length of a path, 0 if not found

Definition at line 325 of file navfn.cpp.

References npath.

◆ getPathX()

float * nav2_navfn_planner::NavFn::getPathX ( )

Accessor for the x-coordinates of a path.

Returns: The x-coordinates of a path

Definition at line 323 of file navfn.cpp.

◆ getPathY()

float * nav2_navfn_planner::NavFn::getPathY ( )

Accessor for the y-coordinates of a path.

Returns: The y-coordinates of a path

Definition at line 324 of file navfn.cpp.

References pathy.

◆ gradCell()

float nav2_navfn_planner::NavFn::gradCell ( int n )

Calculate gradient at a cell.

Parameters

n	Cell number <n>

Returns: float norm calculates gradient at cell <n>, returns norm

Definition at line 935 of file navfn.cpp.

References gradx, grady, ns, and potarr.

Referenced by calcPath().

Here is the caller graph for this function:

◆ initCost()

void nav2_navfn_planner::NavFn::initCost	(	int	k,
		float	v
	)

Initialize cell k with cost v for propagation.

Parameters

k	the cell to initialize
v	the cost to give to the cell

Definition at line 401 of file navfn.cpp.

References potarr.

Referenced by setupNavFn().

Here is the caller graph for this function:

◆ propNavFnAstar()

bool nav2_navfn_planner::NavFn::propNavFnAstar	(	int	cycles,
		std::function< bool()>	cancelChecker
	)

Run propagation for <cycles> iterations, or until start is reached using the best-first A* method with Euclidean distance heuristic.

Parameters

cycles	The maximum number of iterations to run for
cancelChecker	Function to check if the task has been canceled

Returns: true if the start point is reached

Definition at line 660 of file navfn.cpp.

References curT, last_path_cost_, nobs, ns, overP, overPe, pending, potarr, priInc, and updateCellAstar().

Referenced by calcNavFnAstar().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ propNavFnDijkstra()

bool nav2_navfn_planner::NavFn::propNavFnDijkstra	(	int	cycles,
		std::function< bool()>	cancelChecker,
		bool	atStart = `false`
	)

Run propagation for <cycles> iterations, or until start is reached using breadth-first Dijkstra method.

Parameters

cycles	The maximum number of iterations to run for
cancelChecker	Function to check if the task has been canceled
atStart	Whether or not to stop when the start point is reached

Returns: true if the start point is reached

Definition at line 575 of file navfn.cpp.

References curT, nobs, ns, overP, overPe, pending, potarr, priInc, and updateCell().

Referenced by calcNavFnDijkstra().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setCostmap()

void nav2_navfn_planner::NavFn::setCostmap	(	const COSTTYPE *	cmap,
		bool	isROS = `true`,
		bool	allow_unknown = `true`
	)

Set up the cost array for the planner, usually from ROS.

Parameters

cmap	The costmap
isROS	Whether or not the costmap is coming in in ROS format
allow_unknown	Whether or not the planner should be allowed to plan through unknown space

Definition at line 247 of file navfn.cpp.

References costarr.

◆ setGoal()

void nav2_navfn_planner::NavFn::setGoal ( int * goal )

Sets the goal position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start.

goal and start positions

Parameters

goal	the goal position

Definition at line 185 of file navfn.cpp.

◆ setNavArr()

void nav2_navfn_planner::NavFn::setNavArr	(	int	nx,
		int	ny
	)

Sets or resets the size of the map.

Parameters

nx	The x size of the map
ny	The y size of the map

Definition at line 207 of file navfn.cpp.

References costarr, gradx, grady, ns, pending, and potarr.

Referenced by NavFn().

Here is the caller graph for this function:

◆ setStart()

void nav2_navfn_planner::NavFn::setStart ( int * start )

Sets the start position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start.

Parameters

start the start position

Definition at line 193 of file navfn.cpp.

◆ setupNavFn()

void nav2_navfn_planner::NavFn::setupNavFn ( bool keepit = false )

Set up navigation potential arrays for new propagation.

Parameters

keepit whether or not use COST_NEUTRAL

Definition at line 342 of file navfn.cpp.

References costarr, curT, gradx, grady, initCost(), nobs, ns, overP, overPe, pb1, pb3, pending, and potarr.

Referenced by calcNavFnAstar(), and calcNavFnDijkstra().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateCell()

void nav2_navfn_planner::NavFn::updateCell ( int n )

inline

Updates the cell at index n.

propagation

Parameters

n	The index to update

Definition at line 422 of file navfn.cpp.

References costarr, curT, and potarr.

Referenced by propNavFnDijkstra().

Here is the caller graph for this function:

◆ updateCellAstar()

void nav2_navfn_planner::NavFn::updateCellAstar ( int n )

inline

Updates the cell at index n using the A* heuristic.

Parameters

n	The index to update

Definition at line 496 of file navfn.cpp.

References costarr, curT, and potarr.

Referenced by propNavFnAstar().

Here is the caller graph for this function:

Member Data Documentation

◆ costarr

COSTTYPE* nav2_navfn_planner::NavFn::costarr

cell arrays cost array in 2D configuration space

Definition at line 171 of file navfn.hpp.

Referenced by NavFn(), setCostmap(), setNavArr(), setupNavFn(), updateCell(), and updateCellAstar().

◆ curT

float nav2_navfn_planner::NavFn::curT

block priority thresholds current threshold

Definition at line 182 of file navfn.hpp.

Referenced by propNavFnAstar(), propNavFnDijkstra(), setupNavFn(), updateCell(), and updateCellAstar().

◆ gradx

float* nav2_navfn_planner::NavFn::gradx

gradient and paths

Definition at line 254 of file navfn.hpp.

Referenced by calcPath(), gradCell(), NavFn(), setNavArr(), and setupNavFn().

◆ grady

float * nav2_navfn_planner::NavFn::grady

gradient arrays, size of potential array

Definition at line 254 of file navfn.hpp.

Referenced by calcPath(), gradCell(), NavFn(), setNavArr(), and setupNavFn().

◆ last_path_cost_

float nav2_navfn_planner::NavFn::last_path_cost_

Holds the cost of the path found the last time A* was called

Definition at line 259 of file navfn.hpp.

Referenced by getLastPathCost(), and propNavFnAstar().

◆ nobs

int nav2_navfn_planner::NavFn::nobs

number of obstacle cells

Definition at line 174 of file navfn.hpp.

Referenced by propNavFnAstar(), propNavFnDijkstra(), and setupNavFn().

◆ npath

int nav2_navfn_planner::NavFn::npath

number of path points

Definition at line 256 of file navfn.hpp.

Referenced by calcPath(), getPathLen(), and NavFn().

◆ npathbuf

int nav2_navfn_planner::NavFn::npathbuf

size of pathx, pathy buffers

Definition at line 257 of file navfn.hpp.

Referenced by calcPath(), and NavFn().

◆ ns

int nav2_navfn_planner::NavFn::ns

size of grid, in pixels

Definition at line 122 of file navfn.hpp.

Referenced by calcPath(), gradCell(), propNavFnAstar(), propNavFnDijkstra(), setNavArr(), and setupNavFn().

◆ overP

int * nav2_navfn_planner::NavFn::overP

priority buffer block ptrs

Definition at line 178 of file navfn.hpp.

Referenced by propNavFnAstar(), propNavFnDijkstra(), and setupNavFn().

◆ overPe

int nav2_navfn_planner::NavFn::overPe

end points of arrays

Definition at line 179 of file navfn.hpp.

Referenced by propNavFnAstar(), propNavFnDijkstra(), and setupNavFn().

◆ pathStep

float nav2_navfn_planner::NavFn::pathStep

step size for following gradient

Definition at line 275 of file navfn.hpp.

Referenced by calcPath(), and NavFn().

◆ pathy

float * nav2_navfn_planner::NavFn::pathy

path points, as subpixel cell coordinates

Definition at line 255 of file navfn.hpp.

Referenced by calcPath(), getPathY(), and NavFn().

◆ pb1

int* nav2_navfn_planner::NavFn::pb1

block priority buffers

Definition at line 177 of file navfn.hpp.

Referenced by NavFn(), and setupNavFn().

◆ pb3

int * nav2_navfn_planner::NavFn::pb3

storage buffers for priority blocks

Definition at line 177 of file navfn.hpp.

Referenced by NavFn(), and setupNavFn().

◆ pending

bool* nav2_navfn_planner::NavFn::pending

pending cells during propagation

Definition at line 173 of file navfn.hpp.

Referenced by NavFn(), propNavFnAstar(), propNavFnDijkstra(), setNavArr(), and setupNavFn().

◆ potarr

float* nav2_navfn_planner::NavFn::potarr

potential array, navigation function potential

Definition at line 172 of file navfn.hpp.

Referenced by calcPath(), gradCell(), initCost(), NavFn(), propNavFnAstar(), propNavFnDijkstra(), setNavArr(), setupNavFn(), updateCell(), and updateCellAstar().

◆ priInc

float nav2_navfn_planner::NavFn::priInc

priority threshold increment number of cycles between checks for cancellation

Definition at line 183 of file navfn.hpp.

Referenced by NavFn(), propNavFnAstar(), and propNavFnDijkstra().

The documentation for this class was generated from the following files:

nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp
nav2_navfn_planner/src/navfn.cpp

Public Member Functions

Public Attributes

Static Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ NavFn()

Member Function Documentation

◆ calcNavFnAstar()

◆ calcNavFnDijkstra()

◆ calcPath()

◆ getLastPathCost()

◆ getPathLen()

◆ getPathX()

◆ getPathY()

◆ gradCell()

◆ initCost()

◆ propNavFnAstar()

◆ propNavFnDijkstra()

◆ setCostmap()

◆ setGoal()

◆ setNavArr()

◆ setStart()

◆ setupNavFn()

◆ updateCell()

◆ updateCellAstar()

Member Data Documentation

◆ costarr

◆ curT

◆ gradx

◆ grady

◆ last_path_cost_

◆ nobs

◆ npath

◆ npathbuf

◆ ns

◆ overP

◆ overPe

◆ pathStep

◆ pathy

◆ pb1

◆ pb3

◆ pending

◆ potarr

◆ priInc