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, float minClearance=0.F) const
 Find the closest position to a given position that meets a clearance.
 
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
 

Member Typedef Documentation

◆ Grid

using Grid = Eigen::MatrixXf

◆ Index

using Index = Eigen::Array2i

◆ Mask

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

◆ Position

using Position = Eigen::Vector2f

Member Enumeration Documentation

◆ AddMode

enum class AddMode
strong
Enumerator
MIN 
MAX 
MULTIPLY 

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

Member Function Documentation

◆ add() [1/2]

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

◆ add() [2/2]

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

◆ addCostmapWithSameSizeAndPose()

bool addCostmapWithSameSizeAndPose ( const Costmap & other,
AddMode mode )

◆ centerPose()

core::Pose2D centerPose ( ) const
inline

◆ cutToValidBoundingBox()

void cutToValidBoundingBox ( )

◆ excludeUnreachable()

void excludeUnreachable ( const Position & position)

◆ filter()

void filter ( const Filter & filter)

◆ findClosestCollisionFreeVertex()

std::optional< Costmap::Vertex > findClosestCollisionFreeVertex ( const Position & position,
float maxDistance,
float minClearance = 0.F ) const

Find the closest position to a given position that meets a clearance.

Searches in expanding circles up to maxDistance for the closest position whose grid value exceeds minClearance.

The default of 0 reproduces a plain collision test, which is what a recovery search wants: any cell the robot fits in at all. Callers that have to satisfy a downstream clearance check must pass that same clearance here – a cell that is merely not inside an obstacle still counts as a collision to any checker with a safety margin, so projecting against 0 and then testing against a margin can never converge.

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

◆ forEachCell()

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

◆ forEachCellParallel()

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

◆ getGrid()

const Costmap::Grid & getGrid ( ) const

◆ getLocalSceneBounds()

const SceneBounds & getLocalSceneBounds ( ) const
noexcept

◆ getMask()

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

◆ getMutableGrid()

Grid & getMutableGrid ( )
inline

◆ getMutableMask()

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

◆ getValidBoundingBox()

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

◆ isInCollision()

bool isInCollision ( const Position & p) const

◆ isValid()

bool isValid ( const Index & index) const
noexcept

checks whether the cell is masked out

◆ 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

◆ numberOfValidElements()

std::size_t numberOfValidElements ( ) const

◆ optimum()

Costmap::Optimum optimum ( ) const

◆ origin()

const core::Pose2D & origin ( ) const
inline

◆ params()

const Costmap::Parameters & params ( ) const
noexcept

◆ setOrigin()

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

◆ toIndexUnsanitized()

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

◆ toPositionGlobal()

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

◆ toPositionLocal()

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

◆ toVertex()

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

◆ toVertexOrInvalid()

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

◆ validateSizes()

void validateSizes ( ) const

◆ value() [1/2]

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

◆ value() [2/2]

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

◆ 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

Friends And Related Symbol Documentation

◆ CostmapBuilder

friend class CostmapBuilder
friend

◆ SocialCostmapBuilder

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