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

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_
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 100 of file costmap_queue.h.

Member Typedef Documentation

convenience definition for a pointer

Definition at line 145 of file costmap_queue.h.

Constructor & Destructor Documentation

costmap_queue::CostmapQueue::CostmapQueue ( nav_core2::Costmap costmap,
bool  manhattan = false 


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

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 

Lookup pre-computed distances.

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
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 178 of file costmap_queue.h.

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

Add a cell the queue.

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 

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

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.

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

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)

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

cellThe cell to check
True, unless overriden

Reimplemented in costmap_queue::LimitedCostmapQueue.

Definition at line 140 of file costmap_queue.h.

Member Data Documentation

std::vector<std::vector<double> > costmap_queue::CostmapQueue::cached_distances_

Definition at line 185 of file costmap_queue.h.

int costmap_queue::CostmapQueue::cached_max_distance_

Definition at line 186 of file costmap_queue.h.

nav_core2::Costmap& costmap_queue::CostmapQueue::costmap_

Definition at line 157 of file costmap_queue.h.

bool costmap_queue::CostmapQueue::manhattan_

Definition at line 162 of file costmap_queue.h.

nav_grid::VectorNavGrid<unsigned char> costmap_queue::CostmapQueue::seen_

Definition at line 161 of file costmap_queue.h.

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

autogenerated on Sun Jan 10 2021 04:08:29