#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

Detailed Description

Definition at line 23 of file Costmap3D.h.

Member Typedef Documentation

◆ Grid

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

Definition at line 55 of file Costmap3D.h.

◆ Index

using Index = Eigen::Array2i

Definition at line 50 of file Costmap3D.h.

◆ Mask

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

Definition at line 61 of file Costmap3D.h.

◆ Position

using Position = Eigen::Vector2f

Definition at line 51 of file Costmap3D.h.

◆ RotationDegrees

using RotationDegrees = float

Definition at line 58 of file Costmap3D.h.

◆ RotationIndex

using RotationIndex = int

Definition at line 57 of file Costmap3D.h.

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

Definition at line 29 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

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.

Definition at line 198 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ centerPose()

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

inline

Definition at line 177 of file Costmap3D.h.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ closestRotationFromDegrees()

Costmap3D::Rotation closestRotationFromDegrees ( RotationDegrees degrees ) const

Definition at line 186 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ convertToPseudoSDF()

void convertToPseudoSDF ( )

Definition at line 564 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ convertToPseudoSDFForRotation()

void convertToPseudoSDFForRotation ( RotationIndex rot_index )

Definition at line 577 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ costmapForOrientation()

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

Definition at line 537 of file Costmap3D.cpp.

◆ cutToValidBoundingBox()

void cutToValidBoundingBox ( )

Definition at line 413 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ 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

Definition at line 325 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ findClosestValidIndex()

Costmap3D::Index findClosestValidIndex ( const Index & index )

Definition at line 464 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ getGrid()

const Costmap3D::Grid & getGrid ( ) const

Definition at line 286 of file Costmap3D.cpp.

Here is the caller graph for this function:

◆ getLocalSceneBounds()

const SceneBounds & getLocalSceneBounds ( ) const

noexcept

Definition at line 211 of file Costmap3D.cpp.

Here is the caller graph for this function:

◆ getMask()

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

noexcept

Definition at line 365 of file Costmap3D.cpp.

Here is the caller graph for this function:

◆ getMutableGrid()

Grid & getMutableGrid ( )

inline

Definition at line 133 of file Costmap3D.h.

◆ getMutableMask()

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

noexcept

Definition at line 371 of file Costmap3D.cpp.

◆ getSize()

Costmap3D::Index getSize ( ) const

Definition at line 531 of file Costmap3D.cpp.

Here is the caller graph for this function:

◆ getValidBoundingBox()

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

Definition at line 497 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ globalPose()

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

inline

Definition at line 116 of file Costmap3D.h.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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.

Definition at line 309 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ isInCollision()

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

Definition at line 292 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ isMaskedOut()

bool isMaskedOut ( const Index & index ) const

noexcept

Definition at line 126 of file Costmap3D.cpp.

◆ isValid()

bool isValid ( const Index & index ) const

noexcept

checks whether the cell is masked out

Definition at line 89 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isWithinRange()

bool isWithinRange ( const Index & index ) const

noexcept

Definition at line 109 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ numberOfValidElements()

std::size_t numberOfValidElements ( ) const

Definition at line 452 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ optimum()

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

Definition at line 217 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ origin()

const core::Pose2D & origin ( ) const

inline

Definition at line 226 of file Costmap3D.h.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ params()

const Costmap3D::Parameters & params ( ) const

noexcept

Definition at line 280 of file Costmap3D.cpp.

Here is the caller graph for this function:

◆ rotationDegrees()

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

inline

Definition at line 123 of file Costmap3D.h.

◆ rotationFromIndex()

Costmap3D::Rotation rotationFromIndex ( RotationIndex index ) const

Definition at line 168 of file Costmap3D.cpp.

Here is the caller graph for this function:

◆ rotationLocal()

Eigen::Rotation2Df rotationLocal ( const RotationDegrees degrees ) const

inline

Definition at line 110 of file Costmap3D.h.

◆ setOrigin()

void setOrigin ( const core::Pose2D & globalPose )

inline

Definition at line 232 of file Costmap3D.h.

Here is the call graph for this function:

◆ toPositionGlobal()

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

Definition at line 82 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ toPositionLocal()

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

Definition at line 68 of file Costmap3D.cpp.

Here is the caller graph for this function:

◆ toVertex()

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

Definition at line 140 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ value() [1/2]

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

Definition at line 383 of file Costmap3D.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ value() [2/2]

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

Definition at line 402 of file Costmap3D.cpp.

Here is the call graph for this function:

◆ value_ignore_mask()

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

Definition at line 377 of file Costmap3D.cpp.

◆ 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

Definition at line 556 of file Costmap3D.cpp.

Here is the call graph for this function:

Friends And Related Symbol Documentation

◆ Costmap3DBuilder

friend class Costmap3DBuilder

friend

Definition at line 26 of file Costmap3D.h.

◆ smoothing::Costmap3DWrapper

friend class smoothing::Costmap3DWrapper

friend

Definition at line 27 of file Costmap3D.h.

Member Data Documentation

◆ ALL_ORIENTATIONS

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

staticconstexpr

Definition at line 128 of file Costmap3D.h.

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

Detailed Description

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