#include <QueryMethod.h>

Inheritance diagram for QueryMethod:

[legend]

Collaboration diagram for QueryMethod:

[legend]

Public Types
Local Types
enum	GraphSearchAlg { DIJKSTRAS , ASTAR }

Motion Planning Types
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
	QueryMethod ()

	QueryMethod (XMLNode &_node)

virtual	~QueryMethod ()=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)

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 Member Functions
Helpers
virtual void	Reset (RoadmapType *const _r)

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

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

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

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

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

Detailed Description

Base class for all query methods. These objects evaluate a roadmap under construction to see if a planning task has been satisfied.

The planning task is defined by the MPTask's Constraint objects. The query searches the current roadmap and aims to find a connecting path between configurations satisfying the task's start and goal constraints.

Note: The query will consider multiple nodes satisfying the goal constraints and quit after finding the first. This is an incomplete algorithm if your problem has a sequence of non-point goals.

Todo:: Implement A* by accepting a distance metric label for the heuristic function, and expanding the SSSP functions to accept a binary functor heuristic function.

Member Typedef Documentation

◆ RoadmapType

typedef MPBaseObject::RoadmapType QueryMethod::RoadmapType

◆ VertexSet

typedef RoadmapType::VertexSet QueryMethod::VertexSet

◆ VID

typedef RoadmapType::VID QueryMethod::VID

◆ VIDSet

typedef std::unordered_set<size_t> QueryMethod::VIDSet

Member Enumeration Documentation

◆ GraphSearchAlg

enum QueryMethod::GraphSearchAlg

Enumerator
DIJKSTRAS
ASTAR

Constructor & Destructor Documentation

◆ QueryMethod() [1/2]

QueryMethod::QueryMethod ( )

◆ QueryMethod() [2/2]

QueryMethod::QueryMethod ( XMLNode & _node )

◆ ~QueryMethod()

virtual QueryMethod::~QueryMethod ( )

virtualdefault

Member Function Documentation

◆ DynamicPathWeight()

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

protectedvirtual

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

◆ GeneratePath()

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

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.

◆ Initialize()

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

Reimplemented in LazyQuery, and ClearanceQuery.

◆ operator()()

bool QueryMethod::operator() ( )

overridevirtual

Evaluate a roadmap.

Returns: True if this roadmap meets the evaluation criteria.

Implements MapEvaluatorMethod.

◆ PerformSubQuery()

bool QueryMethod::PerformSubQuery	(	const VID	_start,
		const VIDSet &	_goal
	)

protectedvirtual

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.

◆ Print()

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

overridevirtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from MPBaseObject.

Reimplemented in LazyQuery, and ClearanceQuery.

◆ Reset()

void QueryMethod::Reset ( RoadmapType *const _r )

protectedvirtual

Reset the path and list of undiscovered goals.

Parameters

_r	The roadmap to use.

◆ SetDMLabel()

void QueryMethod::SetDMLabel ( const std::string & _dmLabel )

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.

◆ SetEndTime()

void QueryMethod::SetEndTime ( double _end )

Set the end time of the query.

Parameters

_end	The time to use.

◆ SetLastConstraintTime()

void QueryMethod::SetLastConstraintTime ( double _last )

Set the time of the last constraint.

Parameters

_last The time to use.

◆ SetLastGoalConstraintTime()

void QueryMethod::SetLastGoalConstraintTime ( double _time )

Set the time of the last goal constraint.

Parameters

_time The time to use.

◆ SetPathWeightFunction()

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

virtual

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

Parameters

The	path weight function object to use.

◆ SetSearchAlgViaString()

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

protected

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.

◆ SetStartTime()

void QueryMethod::SetStartTime ( double _start )

Set the start time of the query.

Parameters

_start The time to use.

◆ StaticPathWeight()

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

protectedvirtual

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 in LazyQuery, and ClearanceQuery.

◆ TwoVariableQuery()

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

protected

Perform a query between start and goal vertices in the roadmap

Parameters

_start	The start vertex.
_goal	The goal vertex.

Field Documentation

◆ m_dmLabel

std::string QueryMethod::m_dmLabel

protected

The DistanceMetric label.

◆ m_endTime

double QueryMethod::m_endTime {0}

protected

The end time of the query.

◆ m_goalIndex

size_t QueryMethod::m_goalIndex {0}

protected

Index of next unreached goal.

◆ m_lastConstraint

double QueryMethod::m_lastConstraint {0}

protected

The time of the last constraint.

◆ m_lastGoalConstraint

double QueryMethod::m_lastGoalConstraint {0}

protected

The time of the last goal constraint.

◆ m_roadmap

RoadmapType* QueryMethod::m_roadmap {nullptr}

protected

Last roadmap queried.

◆ m_safeIntervalLabel

std::string QueryMethod::m_safeIntervalLabel

protected

The SafeIntervalTool label.

◆ m_searchAlg

GraphSearchAlg QueryMethod::m_searchAlg {DIJKSTRAS}

protected

The sssp algorithm to use.

◆ m_startTime

double QueryMethod::m_startTime {0}

protected

The start time of the query.

◆ m_task

MPTask* QueryMethod::m_task {nullptr}

protected

Last task we looked at.

◆ m_twoVariable

bool QueryMethod::m_twoVariable {false}

protected

Temporary flag to use two varibale state search.

◆ m_weightFunction

SSSPPathWeightFunction<RoadmapType> QueryMethod::m_weightFunction

protected

The function for computing total path weight.

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

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ RoadmapType

◆ VertexSet

◆ VID

◆ VIDSet

Member Enumeration Documentation

◆ GraphSearchAlg

Constructor & Destructor Documentation

◆ QueryMethod() [1/2]

◆ QueryMethod() [2/2]

◆ ~QueryMethod()

Member Function Documentation

◆ DynamicPathWeight()

◆ GeneratePath()

◆ Initialize()

◆ operator()()

◆ PerformSubQuery()

◆ Print()

◆ Reset()

◆ SetDMLabel()

◆ SetEndTime()

◆ SetLastConstraintTime()

◆ SetLastGoalConstraintTime()

◆ SetPathWeightFunction()

◆ SetSearchAlgViaString()

◆ SetStartTime()

◆ StaticPathWeight()

◆ TwoVariableQuery()

Field Documentation

◆ m_dmLabel

◆ m_endTime

◆ m_goalIndex

◆ m_lastConstraint

◆ m_lastGoalConstraint

◆ m_roadmap

◆ m_safeIntervalLabel

◆ m_searchAlg

◆ m_startTime

◆ m_task

◆ m_twoVariable

◆ m_weightFunction