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.
bool	calcNavFnDijkstra (bool atStart=false)
	Caclulates the full navigation function using Dijkstra.
int	calcPath (int n, int *st=NULL)
	Calculates the path for at mose <n> cycles.
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.
int	getPathLen ()
	Accessor for the length of a path.
float *	getPathX ()
	Accessor for the x-coordinates of a path.
float *	getPathY ()
	Accessor for the y-coordinates of a path.
float	gradCell (int n)
void	initCost (int k, float v)
	Initialize cell k with cost v for propagation.
	NavFn (int nx, int ny)
	Constructs the planner.
bool	propNavFnAstar (int cycles)
	Run propagation for <cycles> iterations, or until start is reached using the best-first A* method with Euclidean distance heuristic.
bool	propNavFnDijkstra (int cycles, bool atStart=false)
	Run propagation for <cycles> iterations, or until start is reached using breadth-first Dijkstra method.
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.
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.
void	setNavArr (int nx, int ny)
	Sets or resets the size of the map.
void	setObs ()
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.
void	setupNavFn (bool keepit=false)
void	updateCell (int n)
	Updates the cell at index n.
void	updateCellAstar (int n)
	Updates the cell at index n using the A* heuristic.
	~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	(	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 110 of file navfn.cpp.

navfn::NavFn::~NavFn ( )

Definition at line 143 of file navfn.cpp.

Member Function Documentation

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

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

Caclulates the full navigation function using Dijkstra.

calculates the full navigation function

Definition at line 290 of file navfn.cpp.

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 lenght of the path found calculates path for at most <n> cycles, returns path length, 0 if none

Definition at line 822 of file navfn.cpp.

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

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

Definition at line 1051 of file navfn.cpp.

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

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

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

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

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

calculates gradient at cell <n>, returns norm

Definition at line 992 of file navfn.cpp.

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

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

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

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

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

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

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

void navfn::NavFn::setObs ( )

simple obstacle for testing

Definition at line 362 of file navfn.cpp.

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

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

resets all nav fn arrays for propagation

Definition at line 402 of file navfn.cpp.

void navfn::NavFn::updateCell ( int n ) [inline]

Updates the cell at index n.

propagation

Parameters: