Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
costmap_queue::CostmapQueue Class Reference

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

#include <costmap_queue.h>

Inheritance diagram for costmap_queue::CostmapQueue:
Inheritance graph
[legend]

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...
 
CellDatafront ()
 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::Costmapcostmap_
 
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

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
costmapCostmap which defines the size/number of cells
manhattanIf 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_xThe x coordinate of the current cell
cur_yThe y coordinate of the current cell
src_xThe x coordinate of the source cell
src_yThe 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
xX coordinate of the cell
yY 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
cellThe 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:


costmap_queue
Author(s):
autogenerated on Wed Jun 26 2019 20:06:08