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	setNavPoses (const geometry_msgs::PoseStamped &goal_, const geometry_msgs::PoseStamped &start_, const double resolution_, const double costmap_ox_, const double costmap_oy_)
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	spline ()
	Calculate spline from start to goal without obstacles.
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
double	costmap_ox
double	costmap_oy
int *	curP
int	curPe
float	curT
void(*	displayFn )(NavFn *nav)
int	displayInt
int	goal [2]
float *	gradx
float *	grady
float	last_path_cost_
int	m_goal [2]
int *	nextP
int	nextPe
int	nobs
int	npath
int	npathbuf
int	ns
int	nx
int	ny
COSTTYPE *	obsarr
int *	overP
int	overPe
float	p1 [2]
float	p2 [2]
float	p3 [2]
float	p4 [2]
float	pathStep
float *	pathx
float *	pathy
int *	pb1
int *	pb2
int *	pb3
bool *	pending
float *	potarr
float	priInc
double	resolution
int	S_map [303][2]
int	start [2]
float	theta1
float	theta2
float	theta3
float	theta4
geometry_msgs::PoseStamped	w_goal
geometry_msgs::PoseStamped	w_start
int	world_s

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

Constructor & Destructor Documentation

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

iri_bspline_navfn::NavFn::~NavFn ( )

Definition at line 153 of file navfn.cpp.

Member Function Documentation

bool iri_bspline_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 340 of file navfn.cpp.

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

Caclulates the full navigation function using Dijkstra.

calculates the full navigation function

Definition at line 310 of file navfn.cpp.

int iri_bspline_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 1029 of file navfn.cpp.

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

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

Definition at line 1248 of file navfn.cpp.

float iri_bspline_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 1011 of file navfn.cpp.

int iri_bspline_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 369 of file navfn.cpp.

float * iri_bspline_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 367 of file navfn.cpp.

float * iri_bspline_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 368 of file navfn.cpp.

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

calculates gradient at cell <n>, returns norm

Definition at line 1189 of file navfn.cpp.

void iri_bspline_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 472 of file navfn.cpp.

bool iri_bspline_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 785 of file navfn.cpp.

bool iri_bspline_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 699 of file navfn.cpp.

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

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

Definition at line 1261 of file navfn.cpp.

void iri_bspline_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 244 of file navfn.cpp.

void iri_bspline_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 185 of file navfn.cpp.

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

void iri_bspline_navfn::NavFn::setNavPoses	(	const geometry_msgs::PoseStamped &	goal_,
		const geometry_msgs::PoseStamped &	start_,
		const double	resolution_,
		const double	costmap_ox_,
		const double	costmap_oy_
	)

Definition at line 301 of file navfn.cpp.

void iri_bspline_navfn::NavFn::setObs ( )

simple obstacle for testing

Definition at line 377 of file navfn.cpp.

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

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

resets all nav fn arrays for propagation

Definition at line 417 of file navfn.cpp.

void iri_bspline_navfn::NavFn::spline ( )

Calculate spline from start to goal without obstacles.