A tool for finding the cells closest to some set of originating cells. More...

#include <costmap_queue.h>

Inheritance diagram for costmap_queue::CostmapQueue:

Public Member Functions
	CostmapQueue (nav_core2::Costmap &costmap, bool manhattan=false)
	constructor
void	enqueueCell (unsigned int x, unsigned int y)
	Add a cell the queue.
virtual int	getMaxDistance () const
	Get the maximum x or y distance we'll need to calculate the distance between.
CellData	getNextCell ()
	Get the next cell to examine, and enqueue its neighbors as needed.
void	reset () override
	Clear the queue.
virtual bool	validCellToQueue (const CellData &cell)
	Check to see if we should add this cell to the queue. Always true unless overridden.
Protected Member Functions
void	computeCache ()
	Compute the cached distances.
double	distanceLookup (const unsigned int cur_x, const unsigned int cur_y, const unsigned int src_x, const unsigned int src_y)
	Lookup pre-computed distances.
void	enqueueCell (unsigned int cur_x, unsigned int cur_y, unsigned int src_x, unsigned int src_y)
	Enqueue a cell with the given coordinates and the given source cell.
Protected Attributes
std::vector< std::vector < double > >	cached_distances_
int	cached_max_distance_
nav_core2::Costmap &	costmap_
bool	manhattan_
nav_grid::VectorNavGrid < unsigned char >	seen_

Detailed Description

A tool for finding the cells closest to some set of originating cells.

A common operation with costmaps is to define a set of cells in the costmap, and then perform some operation on all the other cells based on which cell in the original set the other cells are closest to. This operation is done in the inflation layer to figure out how far each cell is from an obstacle, and is also used in a number of Trajectory cost functions.

It is implemented with a queue. The standard operation is to enqueueCell the original set, and then retreive the other cells with the isEmpty/getNextCell iterator-like functionality. getNextCell returns an object that contains the coordinates of this cell and the origin cell, as well as the distance between them. By default, the Euclidean distance is used for ordering, but passing in manhattan=true to the constructor will use the Manhattan distance.

The validCellToQueue overridable-function allows for deriving classes to limit the queue traversal to a subset of all costmap cells. LimitedCostmapQueue does this by ignoring distances above a limit.

Definition at line 99 of file costmap_queue.h.

Constructor & Destructor Documentation

costmap_queue::CostmapQueue::CostmapQueue	(	nav_core2::Costmap &	costmap,
		bool	manhattan = `false`
	)		`[explicit]`

constructor

Parameters:

costmap	Costmap which defines the size/number of cells
manhattan	If true, sort cells by Manhattan distance, otherwise use Euclidean distance

Definition at line 41 of file costmap_queue.cpp.

Member Function Documentation

void costmap_queue::CostmapQueue::computeCache ( ) [protected]

Compute the cached distances.

Definition at line 99 of file costmap_queue.cpp.

double costmap_queue::CostmapQueue::distanceLookup	(	const unsigned int	cur_x,
		const unsigned int	cur_y,
		const unsigned int	src_x,
		const unsigned int	src_y
	)		`[inline, protected]`

Lookup pre-computed distances.

Parameters:

cur_x	The x coordinate of the current cell
cur_y	The y coordinate of the current cell
src_x	The x coordinate of the source cell
src_y	The y coordinate of the source cell

Returns:: Precomputed distance

NB: Note that while abs() has the correct behavior i.e. abs(2 - 5) ==> 3. std::abs() without explicit casting does not have the correct behavior i.e. std::abs(2 - 5) ==> (the unsigned version of) -3. We're using explicit casting here to ensure the behavior is not compiler dependent. std::abs(static_cast<int>(2) - static_cast<int>(5)) ==> 3.

Definition at line 177 of file costmap_queue.h.

void costmap_queue::CostmapQueue::enqueueCell	(	unsigned int	x,
		unsigned int	y
	)

Add a cell the queue.

Parameters:

x	X coordinate of the cell
y	Y coordinate of the cell

Definition at line 55 of file costmap_queue.cpp.

void costmap_queue::CostmapQueue::enqueueCell	(	unsigned int	cur_x,
		unsigned int	cur_y,
		unsigned int	src_x,
		unsigned int	src_y
	)		`[protected]`

Enqueue a cell with the given coordinates and the given source cell.

Definition at line 60 of file costmap_queue.cpp.

virtual int costmap_queue::CostmapQueue::getMaxDistance ( ) const [inline, virtual]

Get the maximum x or y distance we'll need to calculate the distance between.

Reimplemented in costmap_queue::LimitedCostmapQueue.

Definition at line 132 of file costmap_queue.h.

CellData costmap_queue::CostmapQueue::getNextCell ( )

Get the next cell to examine, and enqueue its neighbors as needed.

Returns:: The next cell

NB: Assumes that isEmpty has been called before this call and returned false

Definition at line 75 of file costmap_queue.cpp.

void costmap_queue::CostmapQueue::reset ( ) [override, virtual]

Clear the queue.

Reimplemented from costmap_queue::MapBasedQueue< CellData >.

Definition at line 47 of file costmap_queue.cpp.

virtual bool costmap_queue::CostmapQueue::validCellToQueue ( const CellData & cell ) [inline, virtual]

Check to see if we should add this cell to the queue. Always true unless overridden.

Parameters:

cell	The cell to check

Returns:: True, unless overriden

Reimplemented in costmap_queue::LimitedCostmapQueue.

Definition at line 139 of file costmap_queue.h.

Member Data Documentation

std::vector<std::vector<double> > costmap_queue::CostmapQueue::cached_distances_ [protected]

Definition at line 184 of file costmap_queue.h.

int costmap_queue::CostmapQueue::cached_max_distance_ [protected]

Definition at line 185 of file costmap_queue.h.

nav_core2::Costmap& costmap_queue::CostmapQueue::costmap_ [protected]

Definition at line 156 of file costmap_queue.h.

bool costmap_queue::CostmapQueue::manhattan_ [protected]

Definition at line 161 of file costmap_queue.h.

nav_grid::VectorNavGrid<unsigned char> costmap_queue::CostmapQueue::seen_ [protected]

Definition at line 160 of file costmap_queue.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation