LazyQuery< MPTraits > Class Template Reference

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
MapEvaluator Interface
virtual bool	operator() () override
Query Interface
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)
MapEvaluator Interface
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...
Active Robots
Todo: This is an artifact of developing robot groups. It should be replaced by using a proper subgroup where each robot within is active.
void	SetActiveRobots (const std::vector< size_t > &_activeRobots)
	Set the active robots.
std::vector< size_t >	GetActiveRobots () const
	Get the active robots.
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::RoadmapType	RoadmapType
typedef RoadmapType::VID	VID
typedef RoadmapType::EdgeID	EdgeID
typedef MPTraits::GoalTracker	GoalTracker
typedef GoalTracker::VIDSet	VIDSet
Local Types
enum	GraphSearchAlg { DIJKSTRAS , ASTAR }
Construction
typedef std::unordered_map< size_t, std::unordered_map< size_t, std::vector< Range< double > > > >	EdgeIntervals
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 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
Helpers
bool	ValidatePath ()
bool	PruneInvalidVertices ()
bool	PruneInvalidEdges ()
virtual void	NodeEnhance ()
virtual void	ProcessInvalidNode (const CfgType &_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
Helpers
virtual void	Reset (RoadmapType *const _r)
void	SetSearchAlgViaString (std::string _alg, const std::string &_where)
std::vector< typename QueryMethod< MPTraits >::VID >	TwoVariableQuery (const VID _start, const VIDSet &_goals)

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

Protected Attributes
bool	m_debug
	Print debug info?
MP Object Labels
std::string	m_vcLabel
	The lazy validity checker label.
std::string	m_lpLabel
	The lazy local planner label.
std::string	m_enhanceDmLabel
	The distance metric for enhancement.
std::vector< std::string >	m_ncLabels
	The connectors for enhancement.
Internal State
bool	m_deleteInvalid {true}
	Remove invalid vertices from the roadmap?
std::vector< int >	m_resolutions {1}
	List of resolution multiples to check.
size_t	m_numEnhance {0}
	Number of enhancement nodes to generate.
double	m_d {0}
	Gaussian distance for enhancement sampling.
std::vector< std::pair< CfgType, CfgType > >	m_edges
	Candidate edges for enhancement sampling.
std::unordered_map< RoadmapType *, VertexSet >	m_invalidVertices
	Lazy-invalidated vertices.
std::unordered_map< RoadmapType *, EdgeSet >	m_invalidEdges
	Lazy-invalidated edges.
Internal State
RoadmapType *	m_roadmap {nullptr}
	Last roadmap queried.
MPTask *	m_task {nullptr}
	Last task we looked at.
size_t	m_goalIndex {0}
	Index of next unreached goal.
GraphSearchAlg	m_searchAlg {DIJKSTRAS}
	The sssp algorithm to use.
std::string	m_safeIntervalLabel
	The SafeIntervalTool label.
std::string	m_dmLabel
	The DistanceMetric label.
bool	m_twoVariable {false}
	Temporary flag to use two varibale state search.
SSSPPathWeightFunction< RoadmapType >	m_weightFunction
	The function for computing total path weight.
double	m_lastConstraint {0}
	The time of the last constraint.
double	m_lastGoalConstraint {0}
	The time of the last goal constraint.
double	m_startTime {0}
	The start time of the query.
double	m_endTime {0}
	The end time of the query.
Internal State
std::vector< size_t >	m_activeRobots

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

template<typename MPTraits>
class LazyQuery< MPTraits >

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

Print()

template<typename MPTraits >

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

overridevirtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from QueryMethod< MPTraits >.

Initialize()

template<typename MPTraits >

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

SetPathWeightFunction() [1/2]

template<typename MPTraits >

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

Reset() [1/2]

template<typename MPTraits >

void LazyQuery< MPTraits >::Reset ( RoadmapType *const  _r )

overrideprotectedvirtual

Reset the path and list of undiscovered goals

Parameters

_r	The roadmap to use.

PerformSubQuery()

template<typename MPTraits >

bool LazyQuery< MPTraits >::PerformSubQuery ( const VID  _start, const VIDSet &  _goal  )

overrideprotectedvirtual

Check whether a path connecting a start to one of several goals exists in the roadmap.

Parameters

_start	The start VID to use.
_goals	The goal VIDs to use.

Returns

True if a path from _start to one of _goals was generated.

Reimplemented from QueryMethod< MPTraits >.

StaticPathWeight()

template<typename MPTraits >

double LazyQuery< MPTraits >::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< MPTraits >.

DynamicPathWeight()

template<typename MPTraits >

double LazyQuery< MPTraits >::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< MPTraits >.

ValidatePath()

template<typename MPTraits >

bool LazyQuery< MPTraits >::ValidatePath

protected

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

Returns

True if the path was valid.

PruneInvalidVertices()

template<typename MPTraits >

bool LazyQuery< MPTraits >::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.

PruneInvalidEdges()

template<typename MPTraits >

bool LazyQuery< MPTraits >::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.

NodeEnhance()

template<typename MPTraits >

void LazyQuery< MPTraits >::NodeEnhance

protectedvirtual

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

ProcessInvalidNode()

template<typename MPTraits >

virtual void LazyQuery< MPTraits >::ProcessInvalidNode ( const CfgType &  _cfg )

protectedvirtual

Additional handling of invalid vertices.

Parameters

_cfg	The invalid configuration to handle.

InvalidateVertex()

template<typename MPTraits >

void LazyQuery< MPTraits >::InvalidateVertex ( const VID  _vid )

protected

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

Parameters

_vid	The vertex descriptor.

InvalidateEdge()

template<typename MPTraits >

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

SetVertexInvalidated()

template<typename MPTraits >

void LazyQuery< MPTraits >::SetVertexInvalidated ( const VID  _vid )

protectednoexcept

Set a vertex as invalidated.

Parameters

_vid	The vertex descriptor.

IsVertexInvalidated()

template<typename MPTraits >

bool LazyQuery< MPTraits >::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.

IsEdgeInvalidated()

template<typename MPTraits >

bool LazyQuery< MPTraits >::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.

SetEdgeInvalidated() [1/2]

template<typename MPTraits >

void LazyQuery< MPTraits >::SetEdgeInvalidated ( const EdgeID  _eid )

protectednoexcept

Set an edge as invalidated.

Parameters

_eid	The edge ID.

SetEdgeInvalidated() [2/2]

template<typename MPTraits >

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

operator()()

template<typename MPTraits >

bool QueryMethod< MPTraits >::operator() ( )

overridevirtualinherited

Evaluate a roadmap.

Returns

True if this roadmap meets the evaluation criteria.

Implements MapEvaluatorMethod< MPTraits >.

GeneratePath()

template<typename MPTraits >

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

inherited

Generate a path through the roadmap from a start node to an end node.

Parameters

_start	The start node.
_end	The set of allowed end nodes.

Returns

A path of VIDs which transition from _start to the nearest node in _end.

SetDMLabel()

template<typename MPTraits >

void QueryMethod< MPTraits >::SetDMLabel ( const std::string &  _dmLabel )

inherited

Set an alternate distance metric to use when searching the roadmap (instead of the saved edge weights).

Parameters

_label

The Distance Metric label to use. Set to empty string to use the saved edge weights.

SetPathWeightFunction() [2/2]

template<typename MPTraits >

void QueryMethod< MPTraits >::SetPathWeightFunction ( SSSPPathWeightFunction< RoadmapType >  _f )

virtualinherited

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

Parameters

The	path weight function object to use.

SetLastConstraintTime()

template<typename MPTraits >

void QueryMethod< MPTraits >::SetLastConstraintTime ( double  _last )

inherited

Set the time of the last constraint.

Parameters

_last

The time to use.

SetLastGoalConstraintTime()

template<typename MPTraits >

void QueryMethod< MPTraits >::SetLastGoalConstraintTime ( double  _time )

inherited

Set the time of the last goal constraint.

Parameters

_time

The time to use.

SetStartTime()

template<typename MPTraits >

void QueryMethod< MPTraits >::SetStartTime ( double  _start )

inherited

Set the start time of the query.

Parameters

_start

The time to use.

SetEndTime()

template<typename MPTraits >

void QueryMethod< MPTraits >::SetEndTime ( double  _end )

inherited

Set the end time of the query.

Parameters

_end	The time to use.

Reset() [2/2]

template<typename MPTraits >

void QueryMethod< MPTraits >::Reset ( RoadmapType *const  _r )

protectedvirtualinherited

Reset the path and list of undiscovered goals.

Parameters

_r	The roadmap to use.

SetSearchAlgViaString()

template<typename MPTraits >

void QueryMethod< MPTraits >::SetSearchAlgViaString ( std::string  _alg, const std::string &  _where  )

protectedinherited

Set the search algorithm choice from a string.

Parameters

_alg	The search algorithm to use ('astar' or 'dijkstras').
_where	Error location info in case _alg isn't recognized.

TwoVariableQuery()

template<typename MPTraits >

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

protectedinherited

Perform a query between start and goal vertices in the roadmap

Parameters

_start	The start vertex.
_goal	The goal vertex.

SetEdgeIntervals()

template<typename MPTraits >

virtual void MapEvaluatorMethod< MPTraits >::SetEdgeIntervals ( EdgeIntervals  _edgeIntervals )

virtualinherited

Set the edge intervals of a roadmap.

Reimplemented in SIPPMethod< MPTraits >.

SetMinEndtime()

template<typename MPTraits >

virtual void MapEvaluatorMethod< MPTraits >::SetMinEndtime ( double  _minEndtime )

virtualinherited

Set the minimum end time of a path.

Reimplemented in SIPPMethod< 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

Member Data Documentation

m_activeRobots

template<typename MPTraits >

std::vector<size_t> MapEvaluatorMethod< MPTraits >::m_activeRobots

protectedinherited

The active robots, used only by group map evaluators. Depending on the evaluator, the usage could be different, but the current use case is to set the robot(s) that are being moved, then a clearance check is done only considering those specified bodies (see MinimumClearanceEvaluator).

Todo:

This is an artifact of developing robot groups. It should be replaced by using a proper subgroup where each robot within is active.

---

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

• LazyQuery.h