#include <cost_map.hpp>
Public Types | |
typedef cost_map::Matrix::Scalar | DataType |
typedef cost_map::Matrix | Matrix |
Public Member Functions | |
void | add (const std::string &layer, const DataType value=NO_INFORMATION) |
void | add (const std::string &layer, const Matrix &data) |
bool | addDataFrom (const CostMap &other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector< std::string > layers=std::vector< std::string >()) |
DataType & | at (const std::string &layer, const Index &index) |
DataType | at (const std::string &layer, const Index &index) const |
DataType & | atPosition (const std::string &layer, const Position &position) |
DataType | atPosition (const std::string &layer, const Position &position, grid_map::InterpolationMethods interpolation_method=grid_map::InterpolationMethods::INTER_NEAREST) const |
void | clear (const std::string &layer) |
void | clearAll () |
void | clearBasic () |
CostMap (const std::vector< std::string > &layers) | |
CostMap () | |
bool | erase (const std::string &layer) |
bool | exists (const std::string &layer) const |
bool | extendToInclude (const CostMap &other) |
const Matrix & | get (const std::string &layer) const |
Matrix & | get (const std::string &layer) |
const std::vector< std::string > & | getBasicLayers () const |
const std::string & | getFrameId () const |
bool | getIndex (const Position &position, Index &index) const |
const std::vector< std::string > & | getLayers () const |
const Length & | getLength () const |
bool | getPosition (const Index &index, Position &position) const |
const Position & | getPosition () const |
bool | getPosition3 (const std::string &layer, const Index &index, Position3 &position) const |
double | getResolution () const |
const Size & | getSize () const |
const Index & | getStartIndex () const |
CostMap | getSubmap (const Position &position, const Length &length, bool &isSuccess) const |
CostMap | getSubmap (const Position &position, const Length &length, Index &indexInSubmap, bool &isSuccess) const |
Time | getTimestamp () const |
bool | getVector (const std::string &layerPrefix, const Index &index, Eigen::Vector3d &vector) const |
bool | hasSameLayers (const CostMap &other) const |
bool | isInside (const Position &position) const |
bool | isValid (const Index &index) const |
bool | isValid (const Index &index, const std::string &layer) const |
bool | isValid (const Index &index, const std::vector< std::string > &layers) const |
bool | move (const Position &position, std::vector< BufferRegion > &newRegions) |
bool | move (const Position &position) |
const Matrix & | operator[] (const std::string &layer) const |
Matrix & | operator[] (const std::string &layer) |
void | resetTimestamp () |
void | setBasicLayers (const std::vector< std::string > &basicLayers) |
void | setFrameId (const std::string &frameId) |
void | setGeometry (const Length &length, const double resolution, const Position &position=Position::Zero()) |
void | setGeometry (const SubmapGeometry &geometry) |
void | setPosition (const Position &position) |
void | setStartIndex (const Index &startIndex) |
void | setTimestamp (const Time timestamp) |
virtual | ~CostMap () |
Private Member Functions | |
bool | atPositionLinearInterpolated (const std::string &layer, const Position &position, float &value) const |
void | clearCols (unsigned int index, unsigned int nCols) |
void | clearRows (unsigned int index, unsigned int nRows) |
void | resize (const Eigen::Array2i &bufferSize) |
Private Attributes | |
std::vector< std::string > | basicLayers_ |
std::unordered_map< std::string, Matrix > | data_ |
Grid map data stored as layers of matrices. More... | |
std::string | frameId_ |
Frame id of the grid map. More... | |
std::vector< std::string > | layers_ |
Names of the data layers. More... | |
Length | length_ |
Side length of the map in x- and y-direction [m]. More... | |
Position | position_ |
Map position in the grid map frame [m]. More... | |
double | resolution_ |
Map resolution in xy plane [m/cell]. More... | |
Size | size_ |
Size of the buffer (rows and cols of the data structure). More... | |
Index | startIndex_ |
Circular buffer start indeces. More... | |
Time | timestamp_ |
Timestamp of the grid map (nanoseconds). More... | |
Grid map managing multiple overlaying maps holding float values. Data structure implemented as two-dimensional circular buffer so map can be moved efficiently.
Data is defined with string keys. Examples are:
Definition at line 34 of file cost_map.hpp.
typedef cost_map::Matrix::Scalar cost_map::CostMap::DataType |
Storage element type for this map.
Definition at line 38 of file cost_map.hpp.
Storage container type for this map.
Definition at line 39 of file cost_map.hpp.
cost_map::CostMap::CostMap | ( | const std::vector< std::string > & | layers | ) |
Constructor.
layers | a vector of strings containing the definition/description of the data layer. |
Definition at line 19 of file src/lib/cost_map.cpp.
cost_map::CostMap::CostMap | ( | ) |
Emtpy constructor.
Definition at line 34 of file src/lib/cost_map.cpp.
|
virtual |
Destructor.
Definition at line 39 of file src/lib/cost_map.cpp.
void cost_map::CostMap::add | ( | const std::string & | layer, |
const DataType | value = NO_INFORMATION |
||
) |
Add a new empty data layer.
layer | the name of the layer. value the value to initialize the cells with. |
Definition at line 87 of file src/lib/cost_map.cpp.
void cost_map::CostMap::add | ( | const std::string & | layer, |
const Matrix & | data | ||
) |
Add a new data layer (if the layer already exists, overwrite its data, otherwise add layer and data).
layer | the name of the layer. |
data | the data to be added. |
Definition at line 92 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::addDataFrom | ( | const CostMap & | other, |
bool | extendMap, | ||
bool | overwriteData, | ||
bool | copyAllLayers, | ||
std::vector< std::string > | layers = std::vector<std::string>() |
||
) |
Adds data from an other grid map to this grid map
other | the grid map to take data from. |
extendMap | if true the grid map is resized that the other map fits within. |
overwriteData | if true the new data replaces the old values, else only invalid cells are updated. |
copyAllLayer | if true all layers are used to add data. |
layers | the layers that are copied if not all layers are used. |
Definition at line 401 of file src/lib/cost_map.cpp.
Get cell data for requested index.
layer | the name of the layer to be accessed. |
index | the requested index. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 196 of file src/lib/cost_map.cpp.
Get cell data for requested index. Const version form above.
layer | the name of the layer to be accessed. |
index | the requested index. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 205 of file src/lib/cost_map.cpp.
Get cell data at requested position.
layer | the name of the layer to be accessed. |
position | the requested position. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 161 of file src/lib/cost_map.cpp.
DataType cost_map::CostMap::atPosition | ( | const std::string & | layer, |
const Position & | position, | ||
grid_map::InterpolationMethods | interpolation_method = grid_map::InterpolationMethods::INTER_NEAREST |
||
) | const |
Get cell data at requested position. Const version form above.
layer | the name of the layer to be accessed. |
position | the requested position. |
std::out_of_range | if no map layer with name layer is present. |
std::runtime_error | if the specified interpolation method is not implemented. |
Definition at line 170 of file src/lib/cost_map.cpp.
|
private |
Get cell data at requested position, linearly interpolated from 2x2 cells.
layer | the name of the layer to be accessed. |
position | the requested position. |
value | the data of the cell. |
Definition at line 602 of file src/lib/cost_map.cpp.
void cost_map::CostMap::clear | ( | const std::string & | layer | ) |
Clears all cells (set to NAN) for a layer.
layer | the layer to be cleared. |
Definition at line 559 of file src/lib/cost_map.cpp.
void cost_map::CostMap::clearAll | ( | ) |
Clears all cells of all layers. If basic layers are used, clearBasic() is preferred as it is more efficient. Header information (geometry etc.) remains valid.
Definition at line 575 of file src/lib/cost_map.cpp.
void cost_map::CostMap::clearBasic | ( | ) |
Clears all cells (set to NAN) for all basic layers. Header information (geometry etc.) remains valid.
Definition at line 568 of file src/lib/cost_map.cpp.
|
private |
Clear a number of columns of the grid map.
index | the left index for the columns to be reset. |
nCols | the number of columns to reset. |
Definition at line 592 of file src/lib/cost_map.cpp.
|
private |
Clear a number of rows of the grid map.
index | the upper index for the rows to be reset. |
nRows | the number of rows to reset. |
Definition at line 582 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::erase | ( | const std::string & | layer | ) |
Removes a layer from the grid map.
layer | the name of the layer to be removed. |
Definition at line 140 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::exists | ( | const std::string & | layer | ) | const |
Checks if data layer exists.
layer | the name of the layer. |
Definition at line 107 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::extendToInclude | ( | const CostMap & | other | ) |
Extends the size of the grip map such that the other grid map fits within.
other | the grid map to extend the size to. |
Definition at line 433 of file src/lib/cost_map.cpp.
const cost_map::Matrix & cost_map::CostMap::get | ( | const std::string & | layer | ) | const |
Returns the grid map data for a layer as matrix.
layer | the name of the layer to be returned. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 112 of file src/lib/cost_map.cpp.
cost_map::Matrix & cost_map::CostMap::get | ( | const std::string & | layer | ) |
Returns the grid map data for a layer as non-const. Use this method with care!
layer | the name of the layer to be returned. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 121 of file src/lib/cost_map.cpp.
const std::vector< std::string > & cost_map::CostMap::getBasicLayers | ( | ) | const |
Gets the names of the basic layers.
Definition at line 74 of file src/lib/cost_map.cpp.
const std::string & cost_map::CostMap::getFrameId | ( | ) | const |
Get the frameId of the grid map.
Definition at line 525 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::getIndex | ( | const Position & | position, |
cost_map::Index & | index | ||
) | const |
Gets the corresponding cell index for a position.
[in] | position | the requested position. |
[out] | index | the corresponding index. |
Definition at line 214 of file src/lib/cost_map.cpp.
const std::vector< std::string > & cost_map::CostMap::getLayers | ( | ) | const |
Gets the names of the layers.
Definition at line 156 of file src/lib/cost_map.cpp.
const Eigen::Array2d & cost_map::CostMap::getLength | ( | ) | const |
Get the side length of the grid map.
Definition at line 530 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::getPosition | ( | const Index & | index, |
cost_map::Position & | position | ||
) | const |
Gets the 2d position of cell specified by the index (x, y of cell position) in the grid map frame.
[in] | index | the index of the requested cell. |
[out] | position | the position of the data point in the parent frame. |
Definition at line 219 of file src/lib/cost_map.cpp.
const Eigen::Vector2d & cost_map::CostMap::getPosition | ( | ) | const |
Get the 2d position of the grid map in the grid map frame.
Definition at line 535 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::getPosition3 | ( | const std::string & | layer, |
const Index & | index, | ||
cost_map::Position3 & | position | ||
) | const |
Gets the 3d position of a data point (x, y of cell position & cell value as z) in the grid map frame. This is useful for data layers such as elevation.
layer | the name of the layer to be accessed. |
index | the index of the requested cell. |
position | the position of the data point in the parent frame. |
Definition at line 248 of file src/lib/cost_map.cpp.
double cost_map::CostMap::getResolution | ( | ) | const |
Get the resolution of the grid map.
Definition at line 540 of file src/lib/cost_map.cpp.
const cost_map::Size & cost_map::CostMap::getSize | ( | ) | const |
Get the grid map size (rows and cols of the data structure).
Definition at line 545 of file src/lib/cost_map.cpp.
const cost_map::Index & cost_map::CostMap::getStartIndex | ( | ) | const |
Get the start index of the circular buffer.
Definition at line 554 of file src/lib/cost_map.cpp.
CostMap cost_map::CostMap::getSubmap | ( | const Position & | position, |
const Length & | length, | ||
bool & | isSuccess | ||
) | const |
Gets a submap from the map. The requested submap is specified with the requested location and length.
[in] | position | the requested position of the submap (usually the center). |
[in] | length | the requested length of the submap. |
[out] | isSuccess | true if successful, false otherwise. |
Definition at line 273 of file src/lib/cost_map.cpp.
CostMap cost_map::CostMap::getSubmap | ( | const Position & | position, |
const Length & | length, | ||
cost_map::Index & | indexInSubmap, | ||
bool & | isSuccess | ||
) | const |
Gets a submap from the map. The requested submap is specified with the requested location and length.
[in] | position | the requested position of the submap (usually the center). |
[in] | length | the requested length of the submap. |
[out] | indexInSubmap | the index of the requested position in the submap. |
[out] | isSuccess | true if successful, false otherwise. |
Definition at line 280 of file src/lib/cost_map.cpp.
Time cost_map::CostMap::getTimestamp | ( | ) | const |
Get the timestamp of the grid map.
Definition at line 510 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::getVector | ( | const std::string & | layerPrefix, |
const Index & | index, | ||
Eigen::Vector3d & | vector | ||
) | const |
Gets the 3d vector of three layers with suffixes 'x', 'y', and 'z'.
layerPrefix | the prefix for the layer to bet get as vector. |
index | the index of the requested cell. |
vector | the vector with the values of the data type. |
Definition at line 259 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::hasSameLayers | ( | const CostMap & | other | ) | const |
Checks if another grid map contains the same layers as this grid map. The other grid map could contain more layers than the checked ones. Does not check the selection of basic layers.
other | the other grid map. |
Definition at line 79 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::isInside | ( | const Position & | position | ) | const |
Check if position is within the map boundaries.
position | the position to be checked. |
Definition at line 224 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::isValid | ( | const Index & | index | ) | const |
Checks if the index of all layers defined as basic types are valid, i.e. if all basic types are finite. Returns false
if no basic types are defined.
For cost maps, this currently always returns true (cells are always initialised at zero, not nan).
index | the index to check. |
Definition at line 229 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::isValid | ( | const Index & | index, |
const std::string & | layer | ||
) | const |
Checks if cell at index is a valid (finite) for a certain layer.
For cost maps, this currently always returns true (cells are always initialised at zero, not nan).
index | the index to check. |
layer | the name of the layer to be checked for validity. |
Definition at line 234 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::isValid | ( | const Index & | index, |
const std::vector< std::string > & | layers | ||
) | const |
Checks if cell at index is a valid (finite) for certain layers.
For cost maps, this currently always returns true (cells are always initialised at zero, not nan).
index | the index to check. |
layers | the layers to be checked for validity. |
Definition at line 239 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::move | ( | const Position & | position, |
std::vector< BufferRegion > & | newRegions | ||
) |
Move the grid map w.r.t. to the grid map frame. Use this to move the grid map boundaries without moving the grid map data. Takes care of all the data handling, such that the grid map data is stationary in the grid map frame.
position | the new location of the grid map in the map frame. |
newRegions | the regions of the newly covered / previously uncovered regions of the buffer. |
Definition at line 327 of file src/lib/cost_map.cpp.
bool cost_map::CostMap::move | ( | const Position & | position | ) |
Move the grid map w.r.t. to the grid map frame. Use this to move the grid map boundaries without moving the grid map data. Takes care of all the data handling, such that the grid map data is stationary in the grid map frame.
position | the new location of the grid map in the map frame. |
Definition at line 395 of file src/lib/cost_map.cpp.
const cost_map::Matrix & cost_map::CostMap::operator[] | ( | const std::string & | layer | ) | const |
Returns the grid map data for a layer as matrix.
layer | the name of the layer to be returned. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 130 of file src/lib/cost_map.cpp.
cost_map::Matrix & cost_map::CostMap::operator[] | ( | const std::string & | layer | ) |
Returns the grid map data for a layer as non-const. Use this method with care!
layer | the name of the layer to be returned. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 135 of file src/lib/cost_map.cpp.
void cost_map::CostMap::resetTimestamp | ( | ) |
Resets the timestamp of the grid map (to zero).
Definition at line 515 of file src/lib/cost_map.cpp.
|
private |
Resize the buffer.
bufferSize | the requested buffer size. |
Definition at line 647 of file src/lib/cost_map.cpp.
void cost_map::CostMap::setBasicLayers | ( | const std::vector< std::string > & | basicLayers | ) |
Set the basic layers that need to be valid for a cell to be considered as valid. Also, the basic layers are set to NAN when clearing the cells with clearBasic()
. By default the list of basic layers is empty.
basicLayers | the list of layers that are the basic layers of the map. |
Definition at line 69 of file src/lib/cost_map.cpp.
void cost_map::CostMap::setFrameId | ( | const std::string & | frameId | ) |
Set the frame id of the grid map.
frameId | the frame id to set. |
Definition at line 520 of file src/lib/cost_map.cpp.
void cost_map::CostMap::setGeometry | ( | const Length & | length, |
const double | resolution, | ||
const Position & | position = Position::Zero() |
||
) |
Set the geometry of the grid map. Clears all the data.
length | the side lengths in x, and y-direction of the grid map [m]. |
resolution | the cell size in [m/cell]. |
position | the 2d position of the grid map in the grid map frame [m]. |
Definition at line 43 of file src/lib/cost_map.cpp.
void cost_map::CostMap::setGeometry | ( | const SubmapGeometry & | geometry | ) |
Set the geometry of the grid map from submap geometry information.
geometry | the submap geometry information. |
Definition at line 64 of file src/lib/cost_map.cpp.
|
inline |
Set the position of the grid map.
position | the 2d position of the grid map in the grid map frame [m]. |
Definition at line 76 of file cost_map.hpp.
void cost_map::CostMap::setStartIndex | ( | const Index & | startIndex | ) |
Set the start index of the circular buffer. Use this method with caution!
Definition at line 550 of file src/lib/cost_map.cpp.
void cost_map::CostMap::setTimestamp | ( | const Time | timestamp | ) |
Set the timestamp of the grid map.
timestamp | the timestamp to set (in nanoseconds). |
Definition at line 505 of file src/lib/cost_map.cpp.
|
private |
List of layers from data_
that are the basic grid map layers. This means that for a cell to be valid, all basic layers need to be valid. Also, the basic layers are set to NAN when clearing the map with clear()
.
Definition at line 483 of file cost_map.hpp.
|
private |
Grid map data stored as layers of matrices.
Definition at line 475 of file cost_map.hpp.
|
private |
Frame id of the grid map.
Definition at line 469 of file cost_map.hpp.
|
private |
Names of the data layers.
Definition at line 478 of file cost_map.hpp.
|
private |
Side length of the map in x- and y-direction [m].
Definition at line 486 of file cost_map.hpp.
|
private |
Map position in the grid map frame [m].
Definition at line 492 of file cost_map.hpp.
|
private |
Map resolution in xy plane [m/cell].
Definition at line 489 of file cost_map.hpp.
|
private |
Size of the buffer (rows and cols of the data structure).
Definition at line 495 of file cost_map.hpp.
|
private |
Circular buffer start indeces.
Definition at line 498 of file cost_map.hpp.
|
private |
Timestamp of the grid map (nanoseconds).
Definition at line 472 of file cost_map.hpp.