TopologicalMap< MPTraits > Class Template Reference

Public Member Functions
Construction
	TopologicalMap ()
	TopologicalMap (XMLNode &_node)
virtual	~TopologicalMap ()
MPBaseObject Overrides
virtual void	Initialize () override
Map Queries
const VertexSet *	GetMappedVIDs (RoadmapType *const _r, const WorkspaceRegion *const _region, const size_t _bodyIndex=0) const
const WorkspaceRegion *	GetMappedRegion (RoadmapType *const _r, const VID _vid, const size_t _bodyIndex=0) const
bool	IsPopulated (RoadmapType *const _r, const WorkspaceRegion *const _region, const size_t _bodyIndex=0) const
Region and Cell Location
const WorkspaceRegion *	GetRandomRegion () const
	Let it be a light for you in dark places, when all other lights go out.
const WorkspaceRegion *	LocateRegion (RoadmapType *const _r, const VID _vid, const size_t _bodyIndex=0) const
const WorkspaceRegion *	LocateRegion (const CfgType &_c, const size_t _bodyIndex=0) const
const WorkspaceRegion *	LocateRegion (const Point3d &_p) const
const WorkspaceRegion *	LocateNearestRegion (const CfgType &_c, const size_t _bodyIndex=0) const
const WorkspaceRegion *	LocateNearestRegion (const Point3d &_p) const
size_t	LocateCell (RoadmapType *const _r, const VID _v, const size_t _bodyIndex=0) const
size_t	LocateCell (const CfgType &_c, const size_t _bodyIndex=0) const
size_t	LocateCell (const Point3d &_p) const
Neighborhood Keys
A neighborhood key describes the workspace regions that are occupied by each body of the robot. This is the result of LocateRegion for each body.
NeighborhoodKey	LocateNeighborhood (RoadmapType *const _r, const VID _v) const
NeighborhoodKey	LocateNeighborhood (const CfgType &_c) const
Decomposition Access
const WorkspaceDecomposition *	GetDecomposition () const
	Get the decomposition used by this map.
Inter-Region Distance
double	ApproximateMinimumInnerDistance (const WorkspaceRegion *const _source, const WorkspaceRegion *const _target)
const DistanceMap &	ComputeApproximateMinimumInnerDistances (const WorkspaceRegion *const _source, const double _radius)
I/O
virtual void	Print (std::ostream &_os) const
Name and Label Accessors
const std::string &	GetName () const
	Get the class name for this object.
const std::string &	GetLabel () const
	Get the unique label for this object.
std::string	GetNameAndLabel () const
	Get the unique string identifier for this object "m_name::m_label".
void	SetLabel (const std::string &)
	Set the unique label for this object.
MPLibrary Accessors
void	SetMPLibrary (MPLibrary *) noexcept
	Set the owning MPLibrary.
MPLibrary *	GetMPLibrary () const noexcept
	Get the owning MPLibrary.
bool	IsRunning () const noexcept
	Check the library's running flag.
DistanceMetricPointer	GetDistanceMetric (const std::string &) const noexcept
	Get a distance metric by label from the owning MPLibrary.
ValidityCheckerPointer	GetValidityChecker (const std::string &) const noexcept
	Get a validity checker by label from the owning MPLibrary.
NeighborhoodFinderPointer	GetNeighborhoodFinder (const std::string &) const noexcept
	Get a neighborhood finder by label from the owning MPLibrary.
SamplerPointer	GetSampler (const std::string &) const noexcept
	Get a sampler by label from the owning MPLibrary.
LocalPlannerPointer	GetLocalPlanner (const std::string &) const noexcept
	Get a local planner by label from the owning MPLibrary.
ExtenderPointer	GetExtender (const std::string &) const noexcept
	Get an extender by label from the owning MPLibrary.
PathModifierPointer	GetPathModifier (const std::string &) const noexcept
	Get a path modifier by label from the owning MPLibrary.
ConnectorPointer	GetConnector (const std::string &) const noexcept
	Get a connector by label from the owning MPLibrary.
MetricPointer	GetMetric (const std::string &) const noexcept
	Get a metric by label from the owning MPLibrary.
MapEvaluatorPointer	GetMapEvaluator (const std::string &) const noexcept
	Get a map evaluator by label from the owning MPLibrary.
MPStrategyPointer	GetMPStrategy (const std::string &) const noexcept
	Get a strategy by label from the owning MPLibrary.
MPTraits::MPTools *	GetMPTools () const noexcept
	Get the MPTools container from the owning MPLibrary.
Problem Accessors
MPProblem *	GetMPProblem () const noexcept
	Get the library's current MPProblem.
Environment *	GetEnvironment () const noexcept
	Get the current environment.
MPTask *	GetTask () const noexcept
	Get the current task.
GroupTask *	GetGroupTask () const noexcept
	Get the current group task.
Solution Accessors
MPSolution *	GetMPSolution () const noexcept
RoadmapType *	GetRoadmap (Robot *const _r=nullptr) const noexcept
	Get the current free-space roadmap.
GroupRoadmapType *	GetGroupRoadmap (RobotGroup *const _g=nullptr) const noexcept
	Get the current free-space group roadmap.
RoadmapType *	GetBlockRoadmap (Robot *const _r=nullptr) const noexcept
	Get the current obstacle-space roadmap.
Path *	GetPath (Robot *const _r=nullptr) const noexcept
	Get the current best path.
GroupPath *	GetGroupPath (RobotGroup *const _g=nullptr) const noexcept
	Get the current best group path.
StatClass *	GetStatClass () const noexcept
	Get the current StatClass.
LocalObstacleMap *	GetLocalObstacleMap () const noexcept
	Get the local obstacle map.
GoalTracker *	GetGoalTracker () const noexcept
	Get the goal tracker.

Public Types
Motion Planning Types
typedef MPTraits::CfgType	CfgType
typedef MPTraits::WeightType	WeightType
typedef MPTraits::RoadmapType	RoadmapType
typedef RoadmapType::VID	VID
typedef RoadmapType::VI	VI
typedef RoadmapType::VertexSet	VertexSet
Local Types
typedef std::vector< const WorkspaceRegion * >	NeighborhoodKey
typedef std::unordered_map< const WorkspaceRegion *, double >	DistanceMap
	A map describing the distance to a region from some starting point.
typedef std::map< const WorkspaceRegion *, VertexSet >	OccupancyMap
	A map of the VIDs occupying a given region.
Local Types
typedef MPTraits::GroupRoadmapType	GroupRoadmapType
typedef MPTraits::LocalObstacleMap	LocalObstacleMap
typedef MPTraits::Path	Path
typedef MPTraits::GroupPathType	GroupPath
typedef MPTraits::MPLibrary	MPLibrary
typedef MPTraits::MPSolution	MPSolution
typedef MPTraits::GoalTracker	GoalTracker
typedef MPLibrary::SamplerPointer	SamplerPointer
typedef MPLibrary::LocalPlannerPointer	LocalPlannerPointer
typedef MPLibrary::ExtenderPointer	ExtenderPointer
typedef MPLibrary::PathModifierPointer	PathModifierPointer
typedef MPLibrary::ConnectorPointer	ConnectorPointer
typedef MPLibrary::MetricPointer	MetricPointer
typedef MPLibrary::MapEvaluatorPointer	MapEvaluatorPointer
typedef MPLibrary::MPStrategyPointer	MPStrategyPointer
typedef MPLibrary::DistanceMetricPointer	DistanceMetricPointer
typedef MPLibrary::ValidityCheckerPointer	ValidityCheckerPointer
typedef MPLibrary::NeighborhoodFinderPointer	NeighborhoodFinderPointer

Protected Member Functions
void	SetName (const std::string &_s)
const std::string &	GetBaseFilename () const

Protected Attributes
bool	m_debug
	Print debug info?

Detailed Description

template<typename MPTraits>
class TopologicalMap< MPTraits >

A topological map from roadmap vertices to decomposition cells. The map is updated whenever a vertex is added or removed.

The documentation will frequently refer to VIDs or configurations that are 'contained' within a workspace region or grid cell. In this context, a configuration is contained by a region or cell if it's reference point (used for distance measurement) lies within.

Reference: Read Sandstrom, Andrew Bregger, Ben Smith, Shawna Thomas, and Nancy M. Amato. "Topological Nearest-Neighbor Filtering for Sampling-based Planners". ICRA 2018.

• and - Read Sandstrom, Jory Denny, and Nancy M. Amato. "Asymptotically-Optimal

@WARNING The current implementation assumes that the workspace decomposition will not change once created. If this occurs, the maps must be reset.

Member Function Documentation

Initialize()

template<typename MPTraits >

void TopologicalMap< MPTraits >::Initialize

overridevirtual

Initialize this object for the current MPProblem. This should reset any internal state of the algorithms so that they are ready for execution. It is also the place to initialize any state that depends on the current problem.

Warning

This member will be called for every compiled algorithm in the planning library - even those that will not be used. If an algorithm needs to do expenisve setup, then this method should only set a flag that tells it to do so on first use. The only exceptions are the MPStrategies, which will only have their initialize called on first use.

Reimplemented from MPBaseObject< MPTraits >.

GetMappedVIDs()

template<typename MPTraits >

const MPTraits::RoadmapType::VertexSet * TopologicalMap< MPTraits >::GetMappedVIDs	(	RoadmapType *const	_r,
		const WorkspaceRegion *const	_region,
		const size_t	_bodyIndex = 0
	)		const

Get the set of VIDs that are bucketed within a given region.

Parameters

_region	The region of interest.
_bodyIndex	The body to use.

Returns

The set of VIDs that have body _bodyIndex mapped to _region.

GetMappedRegion()

template<typename MPTraits >

const WorkspaceRegion * TopologicalMap< MPTraits >::GetMappedRegion	(	RoadmapType *const	_r,
		const VID	_vid,
		const size_t	_bodyIndex = 0
	)		const

Get the workspace region to which a given VID is mapped.

Parameters

_vid	The VID of interest.
_bodyIndex	The body to use.

Returns

The workspace region in which body _bodyIndex resides when configured at _vid.

IsPopulated()

template<typename MPTraits >

bool TopologicalMap< MPTraits >::IsPopulated	(	RoadmapType *const	_r,
		const WorkspaceRegion *const	_region,
		const size_t	_bodyIndex = 0
	)		const

Check if a region is populated.

Parameters

_region	The region to check.
_bodyIndex	The body to use.

Returns

True if the region contains any configurations of _bodyIndex.

LocateRegion() [1/3]

template<typename MPTraits >

const WorkspaceRegion * TopologicalMap< MPTraits >::LocateRegion	(	RoadmapType *const	_r,
		const VID	_vid,
		const size_t	_bodyIndex = 0
	)		const

Find the workspace region that contains the reference point for a given configuration and body.

Parameters

_vid	The configuration's VID.
_bodyIndex	The body to use.

Returns

The region containing _bodyIndex at _vid, or null if in obstacle space.

LocateRegion() [2/3]

template<typename MPTraits >

const WorkspaceRegion * TopologicalMap< MPTraits >::LocateRegion	(	const CfgType &	_c,
		const size_t	_bodyIndex = 0
	)		const

Find the workspace region that contains the reference point for a given configuration and body.

Parameters

_c	The configuration.
_bodyIndex	The body to use.

Returns

The region containing _bodyIndex at _c, or null if in obstacle space.

LocateRegion() [3/3]

template<typename MPTraits >

const WorkspaceRegion * TopologicalMap< MPTraits >::LocateRegion

(

const Point3d &

_p

)

const

Find the workspace region that contains a given point.

Parameters

_p	The workspace point.

Returns

The region containing _p, or null if _p is in obstacle space.

LocateNearestRegion() [1/2]

template<typename MPTraits >

const WorkspaceRegion * TopologicalMap< MPTraits >::LocateNearestRegion	(	const CfgType &	_c,
		const size_t	_bodyIndex = 0
	)		const

Try to find the nearest region for a configuration in obstacle space.

Parameters

_c	The configuration in obstacle space.
_bodyIndex	The body to use.

Returns

The nearest region to _p, or null if it cannot be found.

LocateNearestRegion() [2/2]

template<typename MPTraits >

const WorkspaceRegion * TopologicalMap< MPTraits >::LocateNearestRegion

(

const Point3d &

_p

)

const

Try to find the nearest region for a point in obstacle space.

Parameters

_p	The point in obstacle space.

Returns

The nearest region to _p, or null if it cannot be found.

LocateCell() [1/3]

template<typename MPTraits >

size_t TopologicalMap< MPTraits >::LocateCell	(	RoadmapType *const	_r,
		const VID	_v,
		const size_t	_bodyIndex = 0
	)		const

Find the grid cell index that holds a given configuration.

Parameters

_v	The configuration VID.
_bodyIndex	The body to use.

Returns

The index of the grid cell which contains _bodyIndex when configured at _v.

LocateCell() [2/3]

template<typename MPTraits >

size_t TopologicalMap< MPTraits >::LocateCell	(	const CfgType &	_c,
		const size_t	_bodyIndex = 0
	)		const

Find the grid cell index that holds a given configuration.

Parameters

_c	The configuration.
_bodyIndex	The body to use.

Returns

The index of the grid cell which contains _bodyIndex when configured at _c.

LocateCell() [3/3]

template<typename MPTraits >

size_t TopologicalMap< MPTraits >::LocateCell

(

const Point3d &

_p

)

const

Find the grid cell index that holds a workspace point.

Parameters

_p	The workspace point.

Returns

The index of the grid cell which contains _p.

LocateNeighborhood() [1/2]

template<typename MPTraits >

TopologicalMap< MPTraits >::NeighborhoodKey TopologicalMap< MPTraits >::LocateNeighborhood	(	RoadmapType *const	_r,
		const VID	_v
	)		const

Locate the neighborhood that is occupied by a robot in a particular configuration.

LocateNeighborhood() [2/2]

template<typename MPTraits >

TopologicalMap< MPTraits >::NeighborhoodKey TopologicalMap< MPTraits >::LocateNeighborhood

(

const CfgType &

_c

)

const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

ApproximateMinimumInnerDistance()

template<typename MPTraits >

double TopologicalMap< MPTraits >::ApproximateMinimumInnerDistance	(	const WorkspaceRegion *const	_source,
		const WorkspaceRegion *const	_target
	)

Approximate the minimum inner distance between two regions. This estimates the shortest-path distance through free space.

Parameters

_source	The source region.
_target	The target region.

Returns

The approximate minimum distance from _source to _target measured through free workspace. If it hasn't already been computed, infinity will be returned. Passing null as one of the regions will return the max computed frontier distance (0 for none).

ComputeApproximateMinimumInnerDistances()

template<typename MPTraits >

const TopologicalMap< MPTraits >::DistanceMap & TopologicalMap< MPTraits >::ComputeApproximateMinimumInnerDistances	(	const WorkspaceRegion *const	_source,
		const double	_radius
	)

Compute approximate minimum inner cell distances from a source region out to a given radius.

Parameters

_source	The source cell.
_radius	Compute inner distances for cells within this radius.

Print()

template<typename MPTraits >

void MPBaseObject< MPTraits >::Print ( std::ostream &  _os ) const

virtualinherited

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented in ConditionalEvaluator< MPTraits >, PrintMapEvaluation< MPTraits >, MinkowskiDistance< MPTraits >, AdaptiveRRT< MPTraits >, BasicRRTStrategy< MPTraits >, CCsConnector< MPTraits >, ConnectorMethod< MPTraits >, NeighborhoodConnector< MPTraits >, BasicExtender< MPTraits >, ExtenderMethod< MPTraits >, LocalPlannerMethod< MPTraits >, StraightLine< MPTraits >, ComposeEvaluator< MPTraits >, LazyQuery< MPTraits >, QueryMethod< MPTraits >, SIPPMethod< MPTraits >, BasicPRM< MPTraits >, GroupDecoupledStrategy< MPTraits >, MPStrategyMethod< MPTraits >, TogglePRMStrategy< MPTraits >, BruteForceNF< MPTraits >, NeighborhoodFinderMethod< MPTraits >, and UniformRandomSampler< MPTraits >.

SetName()

template<typename MPTraits >

void MPBaseObject< MPTraits >::SetName ( const std::string &  _s )

protectedinherited

Parameters

_s	Class name

GetBaseFilename()

template<typename MPTraits >

const std::string & MPBaseObject< MPTraits >::GetBaseFilename ( ) const

protectedinherited

Returns

base file name from MPProblem

---

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

• TopologicalMap.h