A tool for finding the cells closest to some set of originating cells. More...
#include <costmap_queue.h>
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 precomputed 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 
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 iteratorlike 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 overridablefunction 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 100 of file costmap_queue.h.
using costmap_queue::CostmapQueue::Ptr = std::shared_ptr<CostmapQueue> 
convenience definition for a pointer
Definition at line 145 of file costmap_queue.h.

explicit 
constructor
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.

protected 
Compute the cached distances.
Definition at line 99 of file costmap_queue.cpp.

inlineprotected 
Lookup precomputed distances.
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 
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 178 of file costmap_queue.h.
void costmap_queue::CostmapQueue::enqueueCell  (  unsigned int  x, 
unsigned int  y  
) 
Add a cell the queue.
x  X coordinate of the cell 
y  Y coordinate of the cell 
Definition at line 55 of file costmap_queue.cpp.

protected 
Enqueue a cell with the given coordinates and the given source cell.
Definition at line 60 of file costmap_queue.cpp.

inlinevirtual 
Get the maximum x or y distance we'll need to calculate the distance between.
Reimplemented in costmap_queue::LimitedCostmapQueue.
Definition at line 133 of file costmap_queue.h.
CellData costmap_queue::CostmapQueue::getNextCell  (  ) 
Get the next cell to examine, and enqueue its neighbors as needed.
NB: Assumes that isEmpty has been called before this call and returned false
Definition at line 75 of file costmap_queue.cpp.

overridevirtual 
Clear the queue.
Reimplemented from costmap_queue::MapBasedQueue< CellData >.
Definition at line 47 of file costmap_queue.cpp.

inlinevirtual 
Check to see if we should add this cell to the queue. Always true unless overridden.
cell  The cell to check 
Reimplemented in costmap_queue::LimitedCostmapQueue.
Definition at line 140 of file costmap_queue.h.

protected 
Definition at line 185 of file costmap_queue.h.

protected 
Definition at line 186 of file costmap_queue.h.

protected 
Definition at line 157 of file costmap_queue.h.

protected 
Definition at line 162 of file costmap_queue.h.

protected 
Definition at line 161 of file costmap_queue.h.