List of all members | Classes | Public Types | Public Member Functions | Static Public Member Functions | Friends
Costmap Class Reference

#include <armarx/navigation/algorithms/Costmap.h>

Classes

struct  Filter
 
struct  Optimum
 
struct  Parameters
 
struct  Vertex
 

Public Types

enum class  AddMode { MIN , MAX , MULTIPLY }
 
using Grid = Eigen::MatrixXf
 
using Index = Eigen::Array2i
 
using Mask = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>
 
using Position = Eigen::Vector2f
 

Public Member Functions

bool add (const Costmap &other, AddMode mode, bool keepThis=false)
 
bool add (const Costmap &other, float weight=1.0)
 
bool addCostmapWithSameSizeAndPose (const Costmap &other, AddMode mode)
 
core::Pose2D centerPose () const
 
 Costmap (const Grid &grid, const Parameters &parameters, const SceneBounds &sceneBounds, const std::optional< Mask > &mask=std::nullopt, const core::Pose2D &origin=core::Pose2D::Identity())
 
void cutToValidBoundingBox ()
 
void excludeUnreachable (const Position &position)
 
void filter (const Filter &filter)
 
std::optional< Vertex > findClosestCollisionFreeVertex (const Position &position, float maxDistance) const
 Find the closest collision-free position to a given position.
 
void forEachCell (const std::function< void(const Index &)> &f) const
 
void forEachCellParallel (const std::function< void(const Index &)> &f) const
 
const Grid & getGrid () const
 
const SceneBounds & getLocalSceneBounds () const noexcept
 
const std::optional< Mask > & getMask () const noexcept
 
Grid & getMutableGrid ()
 
std::optional< Costmap::Mask > & getMutableMask () noexcept
 
std::pair< Index, Index > getValidBoundingBox () const
 
bool isInCollision (const Position &p) const
 
bool isValid (const Index &index) const noexcept
 checks whether the cell is masked out
 
Costmap mergeInto (const std::vector< Costmap > &costmaps, const std::vector< float > &weights) const
 Merge a list of costmaps into this costmap.
 
std::size_t numberOfValidElements () const
 
Optimum optimum () const
 
const core::Pose2D & origin () const
 
const Parameters & params () const noexcept
 
void setOrigin (const core::Pose2D &globalPose)
 
std::pair< int, int > toIndexUnsanitized (const Eigen::Vector2f &globalPosition) const
 
Position toPositionGlobal (const Index &index) const
 
Position toPositionLocal (const Index &index) const
 
Vertex toVertex (const Position &globalPosition) const
 
std::optional< Costmap::Vertex > toVertexOrInvalid (const Eigen::Vector2f &globalPosition) const
 
void validateSizes () const
 
std::optional< float > value (const Index &index) const
 
std::optional< float > value (const Position &position) const
 

Static Public Member Functions

static Costmap WithSameDimsAs (const Costmap &other, const Grid &grid, const std::optional< Mask > &mask=std::nullopt)
 Create a costmap with the same dimensions and parameters as the given one.
 

Friends

class CostmapBuilder
 
class SocialCostmapBuilder
 

Detailed Description

Definition at line 17 of file Costmap.h.

Member Typedef Documentation

◆ Grid

using Grid = Eigen::MatrixXf

Definition at line 57 of file Costmap.h.

◆ Index

using Index = Eigen::Array2i

Definition at line 55 of file Costmap.h.

◆ Mask

using Mask = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>

Definition at line 60 of file Costmap.h.

◆ Position

using Position = Eigen::Vector2f

Definition at line 56 of file Costmap.h.

Member Enumeration Documentation

◆ AddMode

enum class AddMode
strong
Enumerator
MIN 
MAX 
MULTIPLY 

Definition at line 133 of file Costmap.h.

Constructor & Destructor Documentation

◆ Costmap()

Costmap ( const Grid & grid,
const Parameters & parameters,
const SceneBounds & sceneBounds,
const std::optional< Mask > & mask = std::nullopt,
const core::Pose2D & origin = core::Pose2D::Identity() )

Definition at line 31 of file Costmap.cpp.

Member Function Documentation

◆ add() [1/2]

bool add ( const Costmap & other,
AddMode mode,
bool keepThis = false )

Definition at line 446 of file Costmap.cpp.

◆ add() [2/2]

bool add ( const Costmap & other,
float weight = 1.0 )

Definition at line 376 of file Costmap.cpp.

◆ addCostmapWithSameSizeAndPose()

bool addCostmapWithSameSizeAndPose ( const Costmap & other,
AddMode mode )

Definition at line 321 of file Costmap.cpp.

◆ centerPose()

core::Pose2D centerPose ( ) const
inline

Definition at line 109 of file Costmap.h.

◆ cutToValidBoundingBox()

void cutToValidBoundingBox ( )

Definition at line 562 of file Costmap.cpp.

◆ excludeUnreachable()

void excludeUnreachable ( const Position & position)

Definition at line 748 of file Costmap.cpp.

◆ filter()

void filter ( const Filter & filter)

Definition at line 244 of file Costmap.cpp.

◆ findClosestCollisionFreeVertex()

std::optional< Costmap::Vertex > findClosestCollisionFreeVertex ( const Position & position,
float maxDistance ) const

Find the closest collision-free position to a given position.

Searches in expanding circles up to maxDistance for the closest position that is not in collision (grid value > 0).

Parameters
positionThe starting position to search from
maxDistanceMaximum search radius [mm]
Returns
std::optional<Vertex> The closest collision-free vertex, or nullopt if none found

Definition at line 860 of file Costmap.cpp.

◆ forEachCell()

void forEachCell ( const std::function< void(const Index &)> & f) const

Definition at line 219 of file Costmap.cpp.

◆ forEachCellParallel()

void forEachCellParallel ( const std::function< void(const Index &)> & f) const

Definition at line 231 of file Costmap.cpp.

◆ getGrid()

const Costmap::Grid & getGrid ( ) const

Definition at line 308 of file Costmap.cpp.

◆ getLocalSceneBounds()

const SceneBounds & getLocalSceneBounds ( ) const
noexcept

Definition at line 165 of file Costmap.cpp.

◆ getMask()

const std::optional< Costmap::Mask > & getMask ( ) const
noexcept

Definition at line 529 of file Costmap.cpp.

◆ getMutableGrid()

Grid & getMutableGrid ( )
inline

Definition at line 96 of file Costmap.h.

◆ getMutableMask()

std::optional< Costmap::Mask > & getMutableMask ( )
noexcept

Definition at line 535 of file Costmap.cpp.

◆ getValidBoundingBox()

std::pair< Costmap::Index, Costmap::Index > getValidBoundingBox ( ) const

Definition at line 715 of file Costmap.cpp.

◆ isInCollision()

bool isInCollision ( const Position & p) const

Definition at line 314 of file Costmap.cpp.

◆ isValid()

bool isValid ( const Index & index) const
noexcept

checks whether the cell is masked out

Definition at line 82 of file Costmap.cpp.

◆ mergeInto()

Costmap mergeInto ( const std::vector< Costmap > & costmaps,
const std::vector< float > & weights ) const

Merge a list of costmaps into this costmap.

Parameters
costmapslist of other costmaps
weights[weights of other costmaps..., weights of this costmap]
Returns
the merged costmap

Definition at line 596 of file Costmap.cpp.

◆ numberOfValidElements()

std::size_t numberOfValidElements ( ) const

Definition at line 671 of file Costmap.cpp.

◆ optimum()

Costmap::Optimum optimum ( ) const

Definition at line 171 of file Costmap.cpp.

◆ origin()

const core::Pose2D & origin ( ) const
inline

Definition at line 171 of file Costmap.h.

◆ params()

const Costmap::Parameters & params ( ) const
noexcept

Definition at line 302 of file Costmap.cpp.

◆ setOrigin()

void setOrigin ( const core::Pose2D & globalPose)
inline

Definition at line 177 of file Costmap.h.

◆ toIndexUnsanitized()

std::pair< int, int > toIndexUnsanitized ( const Eigen::Vector2f & globalPosition) const

Definition at line 116 of file Costmap.cpp.

◆ toPositionGlobal()

Costmap::Position toPositionGlobal ( const Index & index) const

Definition at line 75 of file Costmap.cpp.

◆ toPositionLocal()

Costmap::Position toPositionLocal ( const Index & index) const

Definition at line 61 of file Costmap.cpp.

◆ toVertex()

Costmap::Vertex toVertex ( const Position & globalPosition) const

Definition at line 146 of file Costmap.cpp.

◆ toVertexOrInvalid()

std::optional< Costmap::Vertex > toVertexOrInvalid ( const Eigen::Vector2f & globalPosition) const

Definition at line 132 of file Costmap.cpp.

◆ validateSizes()

void validateSizes ( ) const

Definition at line 46 of file Costmap.cpp.

◆ value() [1/2]

std::optional< float > value ( const Index & index) const

Definition at line 541 of file Costmap.cpp.

◆ value() [2/2]

std::optional< float > value ( const Position & position) const

Definition at line 553 of file Costmap.cpp.

◆ WithSameDimsAs()

Costmap WithSameDimsAs ( const Costmap & other,
const Grid & grid,
const std::optional< Mask > & mask = std::nullopt )
static

Create a costmap with the same dimensions and parameters as the given one.

Only the content (grid and mask) are altered.

Parameters
otherThe costmap providing the dimensions and parameters
grid
mask
Returns
Costmap

Definition at line 854 of file Costmap.cpp.

Friends And Related Symbol Documentation

◆ CostmapBuilder

friend class CostmapBuilder
friend

Definition at line 22 of file Costmap.h.

◆ SocialCostmapBuilder

friend class SocialCostmapBuilder
friend

Definition at line 23 of file Costmap.h.

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