#include <TopologicalMap.h>

Inheritance diagram for TopologicalMap:

Collaboration diagram for TopologicalMap:

Public Types
Local Types
typedef MPBaseObject::WeightType	WeightType

typedef MPBaseObject::RoadmapType	RoadmapType

typedef RoadmapType::VID	VID

typedef RoadmapType::VI	VI

typedef RoadmapType::VertexSet	VertexSet

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

typedef std::map< const WorkspaceRegion *, VertexSet >	OccupancyMap
	A map of the VIDs occupying a given region. More...

Public Types inherited from MPBaseObject
typedef DefaultWeight< Cfg >	WeightType

typedef GenericStateGraph< Cfg, WeightType >	RoadmapType

typedef GroupCfg< RoadmapType >	GroupCfgType

typedef GroupLocalPlan< RoadmapType >	GroupWeightType

typedef GroupRoadmap< GroupCfgType, GroupWeightType >	GroupRoadmapType

Public Member Functions
Construction
	TopologicalMap ()

	TopologicalMap (double gridSize, string decomp_label)

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

const WorkspaceRegion *	LocateRegion (RoadmapType *const _r, const VID _vid, const size_t _bodyIndex=0) const

const WorkspaceRegion *	LocateRegion (const Cfg &_c, const size_t _bodyIndex=0) const

const WorkspaceRegion *	LocateRegion (const Point3d &_p) const

const WorkspaceRegion *	LocateNearestRegion (const Cfg &_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 Cfg &_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 Cfg &_c) const
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More...

Decomposition Access
const WorkspaceDecomposition *	GetDecomposition () const
	Get the decomposition used by this map. More...

Inter-Region Distance
double	ApproximateMinimumInnerDistance (const WorkspaceRegion const _source, const WorkspaceRegion const _target)

const DistanceMap &	ComputeApproximateMinimumInnerDistances (const WorkspaceRegion *const _source, const double _radius)

Public Member Functions inherited from MPBaseObject
	MPBaseObject (const std::string &_label="", const std::string &_name="", bool _debug=false)

	MPBaseObject (XMLNode &_node)

virtual	~MPBaseObject ()

virtual void	Print (std::ostream &_os) const

const std::string &	GetName () const
	Get the class name for this object. More...

const std::string &	GetLabel () const
	Get the unique label for this object. More...

std::string	GetNameAndLabel () const
	Get the unique string identifier for this object "m_name::m_label". More...

void	SetLabel (const std::string &)
	Set the unique label for this object. More...

void	SetMPLibrary (MPLibrary *) noexcept
	Set the owning MPLibrary. More...

MPLibrary *	GetMPLibrary () const noexcept
	Get the owning MPLibrary. More...

bool	IsRunning () const noexcept
	Check the library's running flag. More...

MPProblem *	GetMPProblem () const noexcept
	Get the library's current MPProblem. More...

Environment *	GetEnvironment () const noexcept
	Get the current environment. More...

MPTask *	GetTask () const noexcept
	Get the current task. More...

GroupTask *	GetGroupTask () const noexcept
	Get the current group task. More...

MPSolutionType *	GetMPSolution () const noexcept

RoadmapType *	GetRoadmap (Robot *const _r=nullptr) const noexcept
	Get the current free-space roadmap. More...

GroupRoadmapType *	GetGroupRoadmap (RobotGroup *const _g=nullptr) const noexcept
	Get the current free-space group roadmap. More...

RoadmapType *	GetBlockRoadmap (Robot *const _r=nullptr) const noexcept
	Get the current obstacle-space roadmap. More...

Path *	GetPath (Robot *const _r=nullptr) const noexcept

GroupPath *	GetGroupPath (RobotGroup *const _g=nullptr) const noexcept
	Get the current best group path. More...

StatClass *	GetStatClass () const noexcept
	Get the current StatClass. More...

LocalObstacleMap *	GetLocalObstacleMap () const noexcept
	Get the local obstacle map. More...

Additional Inherited Members
Protected Member Functions inherited from MPBaseObject
void	SetName (const std::string &_s)

const std::string &	GetBaseFilename () const

Protected Attributes inherited from MPBaseObject
bool	m_debug
	Print debug info? More...

Detailed Description

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 Typedef Documentation

◆ DistanceMap

typedef std::unordered_map<const WorkspaceRegion*, double> TopologicalMap::DistanceMap

A map describing the distance to a region from some starting point.

◆ NeighborhoodKey

typedef std::vector<const WorkspaceRegion*> TopologicalMap::NeighborhoodKey

◆ OccupancyMap

typedef std::map<const WorkspaceRegion*, VertexSet> TopologicalMap::OccupancyMap

A map of the VIDs occupying a given region.

◆ RoadmapType

typedef MPBaseObject::RoadmapType TopologicalMap::RoadmapType

◆ VertexSet

typedef RoadmapType::VertexSet TopologicalMap::VertexSet

◆ VI

typedef RoadmapType::VI TopologicalMap::VI

◆ VID

typedef RoadmapType::VID TopologicalMap::VID

◆ WeightType

typedef MPBaseObject::WeightType TopologicalMap::WeightType

Constructor & Destructor Documentation

◆ TopologicalMap() [1/3]

TopologicalMap::TopologicalMap ( )

◆ TopologicalMap() [2/3]

TopologicalMap::TopologicalMap	(	double	gridSize,
		string	decomp_label
	)

inline

◆ TopologicalMap() [3/3]

TopologicalMap::TopologicalMap ( XMLNode & _node )

◆ ~TopologicalMap()

TopologicalMap::~TopologicalMap ( )

virtualdefault

Member Function Documentation

◆ ApproximateMinimumInnerDistance()

double TopologicalMap::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()

const TopologicalMap::DistanceMap & TopologicalMap::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.

Todo:: Refactor this and SSSP code to unify implementations. I think it should be feasible to make an adaptor class to give our grid (which represents an implicit graph) an API that is compatible with the STAPL (explicit) graph.

A search element in the priority queue.

< The visited cell.

< The distance to the nearest search root from cell.

Total ordering by increasing distance.

◆ GetDecomposition()

const WorkspaceDecomposition * TopologicalMap::GetDecomposition ( ) const

inline

Get the decomposition used by this map.

◆ GetMappedRegion()

const WorkspaceRegion * TopologicalMap::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.

◆ GetMappedVIDs()

const TopologicalMap::VertexSet * TopologicalMap::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.

◆ GetRandomRegion()

const WorkspaceRegion * TopologicalMap::GetRandomRegion ( ) const

Let it be a light for you in dark places, when all other lights go out.

◆ Initialize()

void TopologicalMap::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.

Todo:: Move this to the decomposition class, which currently cannot do this job because it does not know its own label.

Todo:: We could probably do this lazily if we use something like a segment tree to find the regions which intersect a cell.

@TODO This exception should probably be a check on whether the robot's largest minimum body radius is larger than the grid resolution, I think probably at least 3x?

Reimplemented from MPBaseObject.

◆ IsPopulated()

bool TopologicalMap::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.

◆ LocateCell() [1/3]

size_t TopologicalMap::LocateCell	(	const Cfg &	_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() [2/3]

size_t TopologicalMap::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.

◆ LocateCell() [3/3]

size_t TopologicalMap::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.

◆ LocateNearestRegion() [1/2]

const WorkspaceRegion * TopologicalMap::LocateNearestRegion	(	const Cfg &	_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]

const WorkspaceRegion * TopologicalMap::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.

◆ LocateNeighborhood() [1/2]

TopologicalMap::NeighborhoodKey TopologicalMap::LocateNeighborhood ( const Cfg & _c ) const

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

◆ LocateNeighborhood() [2/2]

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

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

◆ LocateRegion() [1/3]

const WorkspaceRegion * TopologicalMap::LocateRegion	(	const Cfg &	_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() [2/3]

const WorkspaceRegion * TopologicalMap::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.

◆ LocateRegion() [3/3]

const WorkspaceRegion * TopologicalMap::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.

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

/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/MPTools/TopologicalMap.h
/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/MPTools/TopologicalMap.cpp

Public Types

Public Member Functions

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ DistanceMap

◆ NeighborhoodKey

◆ OccupancyMap

◆ RoadmapType

◆ VertexSet

◆ VI

◆ VID

◆ WeightType

Constructor & Destructor Documentation

◆ TopologicalMap() [1/3]

◆ TopologicalMap() [2/3]

◆ TopologicalMap() [3/3]

◆ ~TopologicalMap()

Member Function Documentation

◆ ApproximateMinimumInnerDistance()

◆ ComputeApproximateMinimumInnerDistances()

◆ GetDecomposition()

◆ GetMappedRegion()

◆ GetMappedVIDs()

◆ GetRandomRegion()

◆ Initialize()

◆ IsPopulated()

◆ LocateCell() [1/3]

◆ LocateCell() [2/3]

◆ LocateCell() [3/3]

◆ LocateNearestRegion() [1/2]

◆ LocateNearestRegion() [2/2]

◆ LocateNeighborhood() [1/2]

◆ LocateNeighborhood() [2/2]

◆ LocateRegion() [1/3]

◆ LocateRegion() [2/3]

◆ LocateRegion() [3/3]