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 Types
using	Ptr = std::shared_ptr< CostmapQueue >
	convenience definition for a pointer More...

Public Member Functions
	CostmapQueue (nav_core2::Costmap &costmap, bool manhattan=false)
	constructor More...

void	enqueueCell (unsigned int x, unsigned int y)
	Add a cell the queue. More...

virtual int	getMaxDistance () const
	Get the maximum x or y distance we'll need to calculate the distance between. More...

CellData	getNextCell ()
	Get the next cell to examine, and enqueue its neighbors as needed. More...

void	reset () override
	Clear the queue. More...

virtual bool	validCellToQueue (const CellData &cell)
	Check to see if we should add this cell to the queue. Always true unless overridden. More...

Public Member Functions inherited from costmap_queue::MapBasedQueue< CellData >
void	enqueue (const double priority, CellDataitem)
	Add a new item to the queue with a set priority. More...

CellData &	front ()
	Return the item at the front of the queue. More...

bool	isEmpty ()
	Check to see if there is anything in the queue. More...

	MapBasedQueue (bool reset_bins=true)
	Default Constructor. More...

void	pop ()
	Remove (and destroy) the item at the front of the queue. More...

Protected Member Functions
void	computeCache ()
	Compute the cached distances. More...

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

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

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_

Protected Attributes inherited from costmap_queue::MapBasedQueue< CellData >
ItemMap	item_bins_

unsigned int	item_count_

ItemMapIterator	iter_

ItemMapIterator	last_insert_iter_

bool	reset_bins_

Additional Inherited Members
Protected Types inherited from costmap_queue::MapBasedQueue< CellData >
using	ItemMap = std::map< double, std::vector< CellData >>

using	ItemMapIterator = typename ItemMap::iterator

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.

Member Typedef Documentation

using costmap_queue::CostmapQueue::Ptr = std::shared_ptr<CostmapQueue>

convenience definition for a pointer

Definition at line 144 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
	)

inlineprotected

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

inlinevirtual

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 ( )

overridevirtual

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 )

inlinevirtual

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 Types

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation