#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>

Classes
struct	Optimum

struct	Parameters

struct	Rotation

struct	Vertex

Public Types
using	Grid = std::vector<Eigen::MatrixXf>

using	Index = Eigen::Array2i

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

using	Position = Eigen::Vector2f

using	RotationDegrees = float

using	RotationIndex = int

Public Member Functions
std::pair< Rotation, Rotation >	bracketingRotationsFromDegrees (RotationDegrees degrees) const
	The two orientation slices bracketing the given continuous rotation.

core::Pose2D	centerPose (std::optional< Rotation > rotation=std::nullopt) const

Rotation	closestRotationFromDegrees (RotationDegrees degrees) const

void	convertToPseudoSDF ()

void	convertToPseudoSDFForRotation (RotationIndex rot_index)

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

Costmap	costmapForOrientation (RotationIndex rotationIndex, bool initializeMask=false) const

void	cutToValidBoundingBox ()

std::optional< Vertex >	findClosestCollisionFreeVertex (const Position &position, RotationDegrees orientationDeg, float maxDistance, float minClearance=0.F) const
	Find the closest position where the robot fits at the given continuous orientation with more than minClearance distance to obstacles.

Index	findClosestValidIndex (const Index &index)

const Grid &	getGrid () const

const SceneBounds &	getLocalSceneBounds () const noexcept

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

Grid &	getMutableGrid ()

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

Index	getSize () const

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

core::Pose2D	globalPose (const Index &index, const RotationDegrees &rotationDeg) const

bool	isFreeWithClearance (const Position &p, RotationDegrees orientationDeg, float clearance=0.F) const
	Whether the robot fits at the given position and continuous orientation with more than the given obstacle clearance.

bool	isInCollision (const Position &p, std::optional< Rotation > rotation=std::nullopt) const

bool	isMaskedOut (const Index &index) const noexcept

bool	isValid (const Index &index) const noexcept
	checks whether the cell is masked out

bool	isWithinRange (const Index &index) const noexcept

std::size_t	numberOfValidElements () const

Optimum	optimum (Rotation rotation=ALL_ORIENTATIONS) const

const core::Pose2D &	origin () const

const Parameters &	params () const noexcept

RotationDegrees	rotationDegrees (const core::Pose2D &pose) const

Rotation	rotationFromIndex (RotationIndex index) const

Eigen::Rotation2Df	rotationLocal (const RotationDegrees degrees) const

void	setOrigin (const core::Pose2D &globalPose)

Position	toPositionGlobal (const Index &index) const

Position	toPositionLocal (const Index &index) const

Vertex	toVertex (const Position &globalPosition) const

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

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

float	value_ignore_mask (const Index &index, const RotationIndex rot_index) const

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

Static Public Attributes
static constexpr Rotation	ALL_ORIENTATIONS = {.index = -1, .degrees = -1}

Friends
class	Costmap3DBuilder

class	smoothing::Costmap3DWrapper

Member Typedef Documentation

◆ Grid

using Grid = std::vector<Eigen::MatrixXf>

◆ Index

using Index = Eigen::Array2i

◆ Mask

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

◆ Position

using Position = Eigen::Vector2f

◆ RotationDegrees

using RotationDegrees = float

◆ RotationIndex

using RotationIndex = int

Constructor & Destructor Documentation

◆ Costmap3D()

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

◆ bracketingRotationsFromDegrees()

std::pair< Costmap3D::Rotation, Costmap3D::Rotation > bracketingRotationsFromDegrees ( RotationDegrees degrees ) const

The two orientation slices bracketing the given continuous rotation.

If the rotation lies exactly on a slice, that slice and its successor are returned. The second slice wraps around to index 0 for rotations in the last segment.

◆ centerPose()

core::Pose2D centerPose ( std::optional< Rotation > rotation = std::nullopt ) const

inline

◆ closestRotationFromDegrees()

Costmap3D::Rotation closestRotationFromDegrees ( RotationDegrees degrees ) const

◆ convertToPseudoSDF()

void convertToPseudoSDF ( )

◆ convertToPseudoSDFForRotation()

void convertToPseudoSDFForRotation ( RotationIndex rot_index )

◆ costmapForOrientation()

Costmap costmapForOrientation	(	RotationIndex	rotationIndex,
		bool	initializeMask = false ) const

◆ cutToValidBoundingBox()

void cutToValidBoundingBox ( )

◆ findClosestCollisionFreeVertex()

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

Find the closest position where the robot fits at the given continuous orientation with more than minClearance distance to obstacles.

The two orientation slices bracketing the orientation are overlapped (element-wise minimum) and the resulting 2D costmap is searched in expanding circles up to maxDistance. Intended for recovery planning when the robot is standing in collision.

Parameters

position	The starting position to search from
orientationDeg	The robot's continuous orientation [deg]
maxDistance	Maximum search radius [mm]
minClearance	Required distance to obstacles [mm]

Returns: The closest vertex fulfilling the clearance, or nullopt if none within range

◆ findClosestValidIndex()

Costmap3D::Index findClosestValidIndex ( const Index & index )

◆ getGrid()

const Costmap3D::Grid & getGrid ( ) const

◆ getLocalSceneBounds()

const SceneBounds & getLocalSceneBounds ( ) const

noexcept

◆ getMask()

const std::optional< Costmap3D::Mask > & getMask ( ) const

noexcept

◆ getMutableGrid()

Grid & getMutableGrid ( )

inline

◆ getMutableMask()

std::optional< Costmap3D::Mask > & getMutableMask ( )

noexcept

◆ getSize()

Costmap3D::Index getSize ( ) const

◆ getValidBoundingBox()

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

◆ globalPose()

core::Pose2D globalPose	(	const Index &	index,
		const RotationDegrees &	rotationDeg ) const

inline

◆ isFreeWithClearance()

bool isFreeWithClearance	(	const Position &	p,
		RotationDegrees	orientationDeg,
		float	clearance = 0.F ) const

Whether the robot fits at the given position and continuous orientation with more than the given obstacle clearance.

Checks the two orientation slices bracketing the continuous orientation, so the result is conservative with respect to the orientation discretization.

◆ isInCollision()

bool isInCollision	(	const Position &	p,
		std::optional< Rotation >	rotation = std::nullopt ) const

◆ isMaskedOut()

bool isMaskedOut ( const Index & index ) const

noexcept

◆ isValid()

bool isValid ( const Index & index ) const

noexcept

checks whether the cell is masked out

◆ isWithinRange()

bool isWithinRange ( const Index & index ) const

noexcept

◆ numberOfValidElements()

std::size_t numberOfValidElements ( ) const

◆ optimum()

Costmap3D::Optimum optimum ( Rotation rotation = ALL_ORIENTATIONS ) const

◆ origin()

const core::Pose2D & origin ( ) const

inline

◆ params()

const Costmap3D::Parameters & params ( ) const

noexcept

◆ rotationDegrees()

RotationDegrees rotationDegrees ( const core::Pose2D & pose ) const

inline

◆ rotationFromIndex()

Costmap3D::Rotation rotationFromIndex ( RotationIndex index ) const

◆ rotationLocal()

Eigen::Rotation2Df rotationLocal ( const RotationDegrees degrees ) const

inline

◆ setOrigin()

void setOrigin ( const core::Pose2D & globalPose )

inline

◆ toPositionGlobal()

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

◆ toPositionLocal()

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

◆ toVertex()

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

◆ value() [1/2]

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

◆ value() [2/2]

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

◆ value_ignore_mask()

float value_ignore_mask	(	const Index &	index,
		const RotationIndex	rot_index ) const

◆ WithSameDimsAs()

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

static

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

Only the content (grid and mask) are altered.

Parameters

other	The Costmap3D providing the dimensions and parameters
grid
mask

Returns: Costmap3D

Friends And Related Symbol Documentation

◆ Costmap3DBuilder

friend class Costmap3DBuilder

friend

◆ smoothing::Costmap3DWrapper

friend class smoothing::Costmap3DWrapper

friend

Member Data Documentation

◆ ALL_ORIENTATIONS

Rotation ALL_ORIENTATIONS = {.index = -1, .degrees = -1}

staticconstexpr

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

armarx/navigation/algorithms/orientation_aware/Costmap3D.h
armarx/navigation/algorithms/orientation_aware/Costmap3D.cpp

Classes

Public Types

Public Member Functions

Static Public Member Functions

Static Public Attributes

Friends

Member Typedef Documentation

◆ Grid

◆ Index

◆ Mask

◆ Position

◆ RotationDegrees

◆ RotationIndex

Constructor & Destructor Documentation

◆ Costmap3D()

Member Function Documentation

◆ bracketingRotationsFromDegrees()

◆ centerPose()

◆ closestRotationFromDegrees()

◆ convertToPseudoSDF()

◆ convertToPseudoSDFForRotation()

◆ costmapForOrientation()

◆ cutToValidBoundingBox()

◆ findClosestCollisionFreeVertex()

◆ findClosestValidIndex()

◆ getGrid()

◆ getLocalSceneBounds()

◆ getMask()

◆ getMutableGrid()

◆ getMutableMask()

◆ getSize()

◆ getValidBoundingBox()

◆ globalPose()

◆ isFreeWithClearance()

◆ isInCollision()

◆ isMaskedOut()

◆ isValid()

◆ isWithinRange()

◆ numberOfValidElements()

◆ optimum()

◆ origin()

◆ params()

◆ rotationDegrees()

◆ rotationFromIndex()

◆ rotationLocal()

◆ setOrigin()

◆ toPositionGlobal()

◆ toPositionLocal()

◆ toVertex()

◆ value() [1/2]

◆ value() [2/2]

◆ value_ignore_mask()

◆ WithSameDimsAs()

Friends And Related Symbol Documentation

◆ Costmap3DBuilder

◆ smoothing::Costmap3DWrapper

Member Data Documentation

◆ ALL_ORIENTATIONS