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 <navfn.h>

Public Member Functions
bool	calcNavFnAstar ()
	Calculates a plan using the A* heuristic, returns true if one is found. More...

bool	calcNavFnDijkstra (bool atStart=false)
	Caclulates the full navigation function using Dijkstra. More...

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

void	display (void fn(NavFn *nav), int n=100)

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

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

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

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

float	gradCell (int n)

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

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

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

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

void	savemap (const char *fname)

void	setCostmap (const COSTTYPE *cmap, bool isROS=true, bool allow_unknown=true)
	Set up the cost array for the planner, usually from ROS 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	setNavArr (int nx, int ny)
	Sets or resets the size of the map. 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	setupNavFn (bool keepit=false)

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...

	~NavFn ()

Public Attributes
COSTTYPE *	costarr

int *	curP

int	curPe

float	curT

void(*	displayFn )(NavFn *nav)

int	displayInt

int	goal [2]

float *	gradx

float *	grady

float	last_path_cost_

int *	nextP

int	nextPe

int	nobs

int	npath

int	npathbuf

int	ns

int	nx

int	ny

int *	overP

int	overPe

float	pathStep

float *	pathx

float *	pathy

int *	pb1

int *	pb2

int *	pb3

bool *	pending

float *	potarr

float	priInc

int	start [2]

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 106 of file navfn.h.

Constructor & Destructor Documentation

◆ NavFn()

navfn::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 142 of file navfn.cpp.

◆ ~NavFn()

navfn::NavFn::~NavFn ( )

Definition at line 175 of file navfn.cpp.

Member Function Documentation

◆ calcNavFnAstar()

bool navfn::NavFn::calcNavFnAstar ( )

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

Returns: True if a plan is found, false otherwise calculates a plan, returns true if found

Definition at line 351 of file navfn.cpp.

◆ calcNavFnDijkstra()

bool navfn::NavFn::calcNavFnDijkstra ( bool atStart = false )

Caclulates the full navigation function using Dijkstra.

calculates the full navigation function

Definition at line 322 of file navfn.cpp.

◆ calcPath()

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

Calculates the path for at mose <n> cycles.

Parameters

n	The maximum number of cycles to run for

Returns: The length of the path found calculates path for at most <n> cycles, returns path length, 0 if none

Definition at line 817 of file navfn.cpp.

◆ display()

void navfn::NavFn::display	(	void	fnNavFn *nav,
		int	n = `100`
	)

display callback <n> is the number of cycles between updates

Definition at line 1046 of file navfn.cpp.

◆ getLastPathCost()

float navfn::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 Return cost of path found the last time A* was called

Definition at line 799 of file navfn.cpp.

◆ getPathLen()

int navfn::NavFn::getPathLen ( )

Accessor for the length of a path.

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

Definition at line 380 of file navfn.cpp.

◆ getPathX()

float * navfn::NavFn::getPathX ( )

Accessor for the x-coordinates of a path.

Returns: The x-coordinates of a path x-coordinates of path

Definition at line 378 of file navfn.cpp.

◆ getPathY()

float * navfn::NavFn::getPathY ( )

Accessor for the y-coordinates of a path.

Returns: The y-coordinates of a path y-coordinates of path

Definition at line 379 of file navfn.cpp.

◆ gradCell()

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

calculates gradient at cell <n>, returns norm

Definition at line 987 of file navfn.cpp.

◆ initCost()

void navfn::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 initialize cell <k> with cost <v>, for propagation

Definition at line 451 of file navfn.cpp.

◆ propNavFnAstar()

bool navfn::NavFn::propNavFnAstar ( int cycles )

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

Returns: true if the start point is reached returns true if start point found

Definition at line 724 of file navfn.cpp.

◆ propNavFnDijkstra()

bool navfn::NavFn::propNavFnDijkstra	(	int	cycles,
		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
atStart	Whether or not to stop when the start point is reached

Returns: true if the start point is reached returns true if start point found or full prop

Definition at line 647 of file navfn.cpp.

◆ savemap()

void navfn::NavFn::savemap ( const char * fname )

save costmap write out costmap and start/goal states as fname.pgm and fname.txt

Definition at line 1059 of file navfn.cpp.

◆ setCostmap()

void navfn::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 sets up the cost map

Definition at line 260 of file navfn.cpp.

◆ setGoal()

void navfn::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 205 of file navfn.cpp.

◆ setNavArr()

void navfn::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 sets or resets the size of the map

Definition at line 225 of file navfn.cpp.

◆ setStart()

void navfn::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 213 of file navfn.cpp.

◆ setupNavFn()

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

resets all nav fn arrays for propagation

Definition at line 397 of file navfn.cpp.

◆ updateCell()

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

inline

Updates the cell at index n

propagation

Parameters

n	The index to update updates the cell at index <n>

Definition at line 472 of file navfn.cpp.

◆ updateCellAstar()

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

inline

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

Parameters

n	The index to update updates the cell at index <n>, uses A* heuristic

Definition at line 557 of file navfn.cpp.

Member Data Documentation

◆ costarr

COSTTYPE* navfn::NavFn::costarr

cell arrays cost array in 2D configuration space

Definition at line 170 of file navfn.h.

◆ curP

int* navfn::NavFn::curP

Definition at line 177 of file navfn.h.

◆ curPe

int navfn::NavFn::curPe

Definition at line 178 of file navfn.h.

◆ curT

float navfn::NavFn::curT

block priority thresholds current threshold

Definition at line 181 of file navfn.h.

◆ displayFn

void(* navfn::NavFn::displayFn) (NavFn *nav)

display function itself

Definition at line 258 of file navfn.h.

◆ displayInt

int navfn::NavFn::displayInt

save second argument of display() above

Definition at line 257 of file navfn.h.

◆ goal

int navfn::NavFn::goal[2]

Definition at line 197 of file navfn.h.

◆ gradx

float* navfn::NavFn::gradx

gradient and paths

Definition at line 237 of file navfn.h.

◆ grady

float * navfn::NavFn::grady

gradient arrays, size of potential array

Definition at line 237 of file navfn.h.

◆ last_path_cost_

float navfn::NavFn::last_path_cost_

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

Definition at line 242 of file navfn.h.

◆ nextP

int * navfn::NavFn::nextP

Definition at line 177 of file navfn.h.

◆ nextPe

int navfn::NavFn::nextPe

Definition at line 178 of file navfn.h.

◆ nobs

int navfn::NavFn::nobs

number of obstacle cells

Definition at line 173 of file navfn.h.

◆ npath

int navfn::NavFn::npath

number of path points

Definition at line 239 of file navfn.h.

◆ npathbuf

int navfn::NavFn::npathbuf

size of pathx, pathy buffers

Definition at line 240 of file navfn.h.

◆ ns

int navfn::NavFn::ns

size of grid, in pixels

Definition at line 124 of file navfn.h.

◆ nx

int navfn::NavFn::nx

Definition at line 124 of file navfn.h.

◆ ny

int navfn::NavFn::ny

Definition at line 124 of file navfn.h.

◆ overP

int * navfn::NavFn::overP

priority buffer block ptrs

Definition at line 177 of file navfn.h.

◆ overPe

int navfn::NavFn::overPe

end points of arrays

Definition at line 178 of file navfn.h.

◆ pathStep

float navfn::NavFn::pathStep

step size for following gradient

Definition at line 253 of file navfn.h.

◆ pathx

float* navfn::NavFn::pathx

Definition at line 238 of file navfn.h.

◆ pathy

float * navfn::NavFn::pathy

path points, as subpixel cell coordinates

Definition at line 238 of file navfn.h.

◆ pb1

int* navfn::NavFn::pb1

block priority buffers

Definition at line 176 of file navfn.h.

◆ pb2

int * navfn::NavFn::pb2

Definition at line 176 of file navfn.h.

◆ pb3

int * navfn::NavFn::pb3

storage buffers for priority blocks

Definition at line 176 of file navfn.h.

◆ pending

bool* navfn::NavFn::pending

pending cells during propagation

Definition at line 172 of file navfn.h.

◆ potarr

float* navfn::NavFn::potarr

potential array, navigation function potential

Definition at line 171 of file navfn.h.

◆ priInc

float navfn::NavFn::priInc

priority threshold increment

Definition at line 182 of file navfn.h.

◆ start

int navfn::NavFn::start[2]

Definition at line 198 of file navfn.h.

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

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ NavFn()

◆ ~NavFn()

Member Function Documentation

◆ calcNavFnAstar()

◆ calcNavFnDijkstra()

◆ calcPath()

◆ display()

◆ getLastPathCost()

◆ getPathLen()

◆ getPathX()

◆ getPathY()

◆ gradCell()

◆ initCost()

◆ propNavFnAstar()

◆ propNavFnDijkstra()

◆ savemap()

◆ setCostmap()

◆ setGoal()

◆ setNavArr()

◆ setStart()

◆ setupNavFn()

◆ updateCell()

◆ updateCellAstar()

Member Data Documentation

◆ costarr

◆ curP

◆ curPe

◆ curT

◆ displayFn

◆ displayInt

◆ goal

◆ gradx

◆ grady

◆ last_path_cost_

◆ nextP

◆ nextPe

◆ nobs

◆ npath

◆ npathbuf

◆ ns

◆ nx

◆ ny

◆ overP

◆ overPe

◆ pathStep

◆ pathx

◆ pathy

◆ pb1

◆ pb2

◆ pb3

◆ pending

◆ potarr

◆ priInc

◆ start