#include <LazyQuery.h>

Inheritance diagram for LazyQuery:

[legend]

Collaboration diagram for LazyQuery:

[legend]

Public Types
Motion Planning Types
typedef MPBaseObject::RoadmapType	RoadmapType

typedef RoadmapType::VID	VID

typedef RoadmapType::EdgeID	EdgeID

typedef std::unordered_set< VID >	VIDSet

Public Types inherited from QueryMethod
enum	GraphSearchAlg { DIJKSTRAS , ASTAR }

typedef MPBaseObject::RoadmapType	RoadmapType

typedef RoadmapType::VID	VID

typedef RoadmapType::VertexSet	VertexSet

typedef std::unordered_set< size_t >	VIDSet

Public Types inherited from MapEvaluatorMethod
typedef std::unordered_map< size_t, std::unordered_map< size_t, std::vector< Range< double > > > >	EdgeIntervals

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

	LazyQuery (XMLNode &_node)

virtual	~LazyQuery ()=default

MPBaseObject Overrides
virtual void	Print (std::ostream &_os) const override

virtual void	Initialize () override

Public Member Functions inherited from QueryMethod
	QueryMethod ()

	QueryMethod (XMLNode &_node)

virtual	~QueryMethod ()=default

virtual bool	operator() () override

std::vector< VID >	GeneratePath (const VID _start, const VIDSet &_end)

void	SetDMLabel (const std::string &_dmLabel)

virtual void	SetPathWeightFunction (SSSPPathWeightFunction< RoadmapType > _f)

void	SetLastConstraintTime (double _last)

void	SetLastGoalConstraintTime (double _time)

void	SetStartTime (double _start)

void	SetEndTime (double _end)

Public Member Functions inherited from MapEvaluatorMethod
virtual void	SetEdgeIntervals (EdgeIntervals _edgeIntervals)
	Set the edge intervals of a roadmap. More...

virtual void	SetMinEndtime (double _minEndtime)
	Set the minimum end time of a path. More...

void	SetActiveRobots (const std::vector< size_t > &_activeRobots)
	Set the active robots. More...

std::vector< size_t >	GetActiveRobots () const
	Get the active robots. More...

	MapEvaluatorMethod ()=default

	MapEvaluatorMethod (XMLNode &_node)

virtual	~MapEvaluatorMethod ()=default

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

	MPBaseObject (XMLNode &_node)

virtual	~MPBaseObject ()

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

Protected Types
Internal Types
typedef std::unordered_set< VID >	VertexSet

typedef std::unordered_set< EdgeID >	EdgeSet

Protected Member Functions
Helpers
bool	ValidatePath ()

bool	PruneInvalidVertices ()

bool	PruneInvalidEdges ()

virtual void	NodeEnhance ()

virtual void	ProcessInvalidNode (const Cfg &_cfg)

void	InvalidateVertex (const VID _vid)

void	InvalidateEdge (const VID _source, const VID _target)

Lazy Invalidation
void	SetVertexInvalidated (const VID _vid) noexcept

bool	IsVertexInvalidated (const VID _vid) const noexcept

bool	IsEdgeInvalidated (const EdgeID _eid) const noexcept

bool	IsEdgeInvalidated (const VID _source, const VID _target) const noexcept

void	SetEdgeInvalidated (const EdgeID _eid) noexcept

void	SetEdgeInvalidated (const VID _source, const VID _target) noexcept

Protected Member Functions inherited from QueryMethod
virtual void	Reset (RoadmapType *const _r)

void	SetSearchAlgViaString (std::string _alg, const std::string &_where)

virtual bool	PerformSubQuery (const VID _start, const VIDSet &_goal)

std::vector< typename QueryMethod::VID >	TwoVariableQuery (const VID _start, const VIDSet &_goals)

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

const std::string &	GetBaseFilename () const

Protected Attributes
MP Object Labels
std::string	m_vcLabel
	The lazy validity checker label. More...

std::string	m_lpLabel
	The lazy local planner label. More...

std::string	m_enhanceDmLabel
	The distance metric for enhancement. More...

std::vector< std::string >	m_ncLabels
	The connectors for enhancement. More...

Internal State
bool	m_deleteInvalid {true}
	Remove invalid vertices from the roadmap? More...

std::vector< int >	m_resolutions {1}
	List of resolution multiples to check. More...

size_t	m_numEnhance {0}
	Number of enhancement nodes to generate. More...

double	m_d {0}
	Gaussian distance for enhancement sampling. More...

std::vector< std::pair< Cfg, Cfg > >	m_edges
	Candidate edges for enhancement sampling. More...

std::unordered_map< RoadmapType *, VertexSet >	m_invalidVertices
	Lazy-invalidated vertices. More...

std::unordered_map< RoadmapType *, EdgeSet >	m_invalidEdges
	Lazy-invalidated edges. More...

Protected Attributes inherited from QueryMethod
RoadmapType *	m_roadmap {nullptr}
	Last roadmap queried. More...

MPTask *	m_task {nullptr}
	Last task we looked at. More...

size_t	m_goalIndex {0}
	Index of next unreached goal. More...

GraphSearchAlg	m_searchAlg {DIJKSTRAS}
	The sssp algorithm to use. More...

std::string	m_safeIntervalLabel
	The SafeIntervalTool label. More...

std::string	m_dmLabel
	The DistanceMetric label. More...

bool	m_twoVariable {false}
	Temporary flag to use two varibale state search. More...

SSSPPathWeightFunction< RoadmapType >	m_weightFunction
	The function for computing total path weight. More...

double	m_lastConstraint {0}
	The time of the last constraint. More...

double	m_lastGoalConstraint {0}
	The time of the last goal constraint. More...

double	m_startTime {0}
	The start time of the query. More...

double	m_endTime {0}
	The end time of the query. More...

Protected Attributes inherited from MapEvaluatorMethod
std::vector< size_t >	m_activeRobots

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

QueryMethod Overrides
virtual void	SetPathWeightFunction (SSSPPathWeightFunction< RoadmapType > _f) override

virtual void	Reset (RoadmapType *const _r) override

virtual bool	PerformSubQuery (const VID _start, const VIDSet &_goals) override

virtual double	StaticPathWeight (typename RoadmapType::adj_edge_iterator &_ei, const double _sourceDistance, const double _targetDistance) const override

virtual double	DynamicPathWeight (typename RoadmapType::adj_edge_iterator &_ei, const double _sourceDistance, const double _targetDistance) const override

Detailed Description

First assumes all nodes and edges are valid, then checks for validity of the nodes/edges used in the path.

Reference: Robert Bohlin and Lydia E. Kavraki. "Path Planning Using Lazy PRM". ICRA 2000.

Note: Node enhancement does not work like in the paper. Here we use a flat gaussian distribution with fixed distance.

Member Typedef Documentation

◆ EdgeID

typedef RoadmapType::EdgeID LazyQuery::EdgeID

◆ EdgeSet

typedef std::unordered_set<EdgeID> LazyQuery::EdgeSet

protected

◆ RoadmapType

typedef MPBaseObject::RoadmapType LazyQuery::RoadmapType

◆ VertexSet

typedef std::unordered_set<VID> LazyQuery::VertexSet

protected

◆ VID

typedef RoadmapType::VID LazyQuery::VID

◆ VIDSet

typedef std::unordered_set<VID> LazyQuery::VIDSet

Constructor & Destructor Documentation

◆ LazyQuery() [1/2]

LazyQuery::LazyQuery ( )

◆ LazyQuery() [2/2]

LazyQuery::LazyQuery ( XMLNode & _node )

◆ ~LazyQuery()

virtual LazyQuery::~LazyQuery ( )

virtualdefault

Member Function Documentation

◆ DynamicPathWeight()

double LazyQuery::DynamicPathWeight	(	typename RoadmapType::adj_edge_iterator &	_ei,
		const double	_sourceDistance,
		const double	_targetDistance
	)		const

overrideprotectedvirtual

Define a function for computing path weights w.r.t. dynamic obstacles. Here the metric is the number of time steps, and we return infinity if taking an edge would result in a collision with a dynamic obstacle.

Parameters

_ei	An iterator to the edge we are checking.
_sourceDistance	The shortest time to the source node.
_targetDistance	The best known time to the target node.

Returns: The time to the target node via this edge, or infinity if taking this edge would result in a collision with dynamic obstacles.

Reimplemented from QueryMethod.

◆ Initialize()

void LazyQuery::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 QueryMethod.

◆ InvalidateEdge()

void LazyQuery::InvalidateEdge	(	const VID	_source,
		const VID	_target
	)

protected

Invalidate or delete a roadmap edge according to the deletion option.

Parameters

_source	The source vertex descriptor.
_target	The target vertex descriptor.

◆ InvalidateVertex()

void LazyQuery::InvalidateVertex ( const VID _vid )

protected

Invalidate or delete a roadmap configuration according to the deletion option.

Parameters

_vid	The vertex descriptor.

◆ IsEdgeInvalidated() [1/2]

bool LazyQuery::IsEdgeInvalidated ( const EdgeID _eid ) const

protectednoexcept

Check if an edge is lazily invalidated.

Parameters

_eid	The edge ID.

Returns: True if _eid is lazily invalidated.

◆ IsEdgeInvalidated() [2/2]

bool LazyQuery::IsEdgeInvalidated	(	const VID	_source,
		const VID	_target
	)		const

protectednoexcept

◆ IsVertexInvalidated()

bool LazyQuery::IsVertexInvalidated ( const VID _vid ) const

protectednoexcept

Check if a vertex is lazily invalidated.

Parameters

_vid	The vertex descriptor.

Returns: True if _vid is lazily invalidated.

◆ NodeEnhance()

void LazyQuery::NodeEnhance ( )

protectedvirtual

Choose a random deleted edge and generate nodes with a gaussian distribution around the edge's midpoint.

◆ PerformSubQuery()

bool LazyQuery::PerformSubQuery	(	const VID	_start,
		const VIDSet &	_goals
	)

overrideprotectedvirtual

◆ Print()

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

overridevirtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from QueryMethod.

◆ ProcessInvalidNode()

virtual void LazyQuery::ProcessInvalidNode ( const Cfg & _cfg )

inlineprotectedvirtual

Additional handling of invalid vertices.

Parameters

_cfg	The invalid configuration to handle.

◆ PruneInvalidEdges()

bool LazyQuery::PruneInvalidEdges ( )

protected

Check each edge and ensure it is valid. Upon discovering an invalid edge, delete it and return.

Returns: True if an edge was deleted.

◆ PruneInvalidVertices()

bool LazyQuery::PruneInvalidVertices ( )

protected

Check each vertex and ensure it is valid. Upon discovering an invalid vertex, delete it and return.

Returns: True if a vertex was deleted.

◆ Reset()

void LazyQuery::Reset ( RoadmapType *const _r )

overrideprotectedvirtual

Reset the path and list of undiscovered goals

Parameters

_r	The roadmap to use.

◆ SetEdgeInvalidated() [1/2]

void LazyQuery::SetEdgeInvalidated ( const EdgeID _eid )

protectednoexcept

Set an edge as invalidated.

Parameters

_eid	The edge ID.

◆ SetEdgeInvalidated() [2/2]

void LazyQuery::SetEdgeInvalidated	(	const VID	_source,
		const VID	_target
	)

protectednoexcept

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

Parameters

_source	The VID of the source vertex.
_target	The VID of the target vertex.

◆ SetPathWeightFunction()

void LazyQuery::SetPathWeightFunction ( SSSPPathWeightFunction< RoadmapType > _f )

overridevirtual

Set an alternate path weight function to use when searching the roadmap

Parameters

_f	The path weight function to use.

◆ SetVertexInvalidated()

void LazyQuery::SetVertexInvalidated ( const VID _vid )

protectednoexcept

Set a vertex as invalidated.

Parameters

_vid	The vertex descriptor.

◆ StaticPathWeight()

double LazyQuery::StaticPathWeight	(	typename RoadmapType::adj_edge_iterator &	_ei,
		const double	_sourceDistance,
		const double	_targetDistance
	)		const

overrideprotectedvirtual

Define a function for computing a path weight for a specific edge, ignoring dynamic obstacles.

Parameters

_ei	An iterator to the edge we are checking.
_sourceDistance	The shortest distance to the source node.
_targetDistance	The best known distance to the target node.

Returns: The distance to the target node via this edge, or infinity if the edge isn't used due to lazy invalidation.

Reimplemented from QueryMethod.

◆ ValidatePath()

bool LazyQuery::ValidatePath ( )

protected

Checks validity of nodes and edges and deletes any invalid ones.

Returns: True if the path was valid.

Field Documentation

◆ m_d

double LazyQuery::m_d {0}

protected

Gaussian distance for enhancement sampling.

◆ m_deleteInvalid

bool LazyQuery::m_deleteInvalid {true}

protected

Remove invalid vertices from the roadmap?

◆ m_edges

std::vector<std::pair<Cfg, Cfg> > LazyQuery::m_edges

protected

Candidate edges for enhancement sampling.

◆ m_enhanceDmLabel

std::string LazyQuery::m_enhanceDmLabel

protected

The distance metric for enhancement.

◆ m_invalidEdges

std::unordered_map<RoadmapType*, EdgeSet> LazyQuery::m_invalidEdges

protected

Lazy-invalidated edges.

◆ m_invalidVertices

std::unordered_map<RoadmapType*, VertexSet> LazyQuery::m_invalidVertices

protected

Lazy-invalidated vertices.

◆ m_lpLabel

std::string LazyQuery::m_lpLabel

protected

The lazy local planner label.

◆ m_ncLabels

std::vector<std::string> LazyQuery::m_ncLabels

protected

The connectors for enhancement.

◆ m_numEnhance

size_t LazyQuery::m_numEnhance {0}

protected

Number of enhancement nodes to generate.

◆ m_resolutions

std::vector<int> LazyQuery::m_resolutions {1}

protected

List of resolution multiples to check.

◆ m_vcLabel

std::string LazyQuery::m_vcLabel

protected

The lazy validity checker label.

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

/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/MapEvaluators/LazyQuery.h
/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/MapEvaluators/LazyQuery.cpp

Public Types

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

QueryMethod Overrides

Detailed Description

Member Typedef Documentation

◆ EdgeID

◆ EdgeSet

◆ RoadmapType

◆ VertexSet

◆ VID

◆ VIDSet

Constructor & Destructor Documentation

◆ LazyQuery() [1/2]

◆ LazyQuery() [2/2]

◆ ~LazyQuery()

Member Function Documentation

◆ DynamicPathWeight()

◆ Initialize()

◆ InvalidateEdge()

◆ InvalidateVertex()

◆ IsEdgeInvalidated() [1/2]

◆ IsEdgeInvalidated() [2/2]

◆ IsVertexInvalidated()

◆ NodeEnhance()

◆ PerformSubQuery()

◆ Print()

◆ ProcessInvalidNode()

◆ PruneInvalidEdges()

◆ PruneInvalidVertices()

◆ Reset()

◆ SetEdgeInvalidated() [1/2]

◆ SetEdgeInvalidated() [2/2]

◆ SetPathWeightFunction()

◆ SetVertexInvalidated()

◆ StaticPathWeight()

◆ ValidatePath()

Field Documentation

◆ m_d

◆ m_deleteInvalid

◆ m_edges

◆ m_enhanceDmLabel

◆ m_invalidEdges

◆ m_invalidVertices

◆ m_lpLabel

◆ m_ncLabels

◆ m_numEnhance

◆ m_resolutions

◆ m_vcLabel