#include <SIPPMethod.h>

Inheritance diagram for SIPPMethod:

Collaboration diagram for SIPPMethod:

[legend]

Public Types
Local Types
typedef std::unordered_set< size_t >	VIDSet

typedef MPBaseObject::RoadmapType	RoadmapType

typedef MPBaseObject::GroupRoadmapType	GroupRoadmapType

typedef MPBaseObject::GroupCfgType	GroupCfgType

typedef MPBaseObject::GroupWeightType	GroupWeightType

typedef std::map< std::pair< size_t, size_t >, std::vector< Range< size_t > > >	EdgeIntervalMap

typedef std::map< size_t, std::vector< Range< size_t > > >	VertexIntervalMap

typedef GenericStateGraph< SIPPVertex, SIPPEdge >	SIPPGraph

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

	SIPPMethod (XMLNode &_node)

virtual	~SIPPMethod ()=default

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

virtual void	Initialize () override

MapEvaluator Interface
virtual bool	operator() () override

Query Interface
std::pair< std::vector< size_t >, std::vector< size_t > >	GeneratePath (const size_t _start, const std::vector< size_t > _goals)

void	SetDMLabel (const std::string &_dmLabel)
	Set the distance metric to use. More...

void	SetStartTime (size_t _start)
	Set the start time of the query. More...

void	SetMinEndTime (size_t _end)
	Set the end time of the query. More...

void	SetEdgeIntervals (EdgeIntervalMap _edgeIntervals)
	Set the edge intervals to use to generate a path. More...

void	SetVertexIntervals (VertexIntervalMap _vertexIntervals)
	Set the edge intervals to use to generate a path. More...

bool	SatisfyConstraints (Range< size_t > _interval)
	Check if the path satisfies all constraints. More...

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

virtual bool	PerformSubQuery (const size_t _start, const std::vector< size_t > _goals)

virtual double	PathWeight (typename SIPPGraph::adj_edge_iterator &_ei, const double _sourceDistance, const double _targetDistance)

double	SIPPHeuristic (const SIPPGraph *_g, typename SIPPGraph::vertex_descriptor _source, typename SIPPGraph::vertex_descriptor _target)

void	SIPPNeighbors (SIPPGraph *_g, typename SIPPGraph::vertex_descriptor _vid)
	Neighbors function for Safe interval path planning. More...

template<typename AbstractRoadmap >
void	SIPPNeighbors (SIPPGraph _g, typename SIPPGraph::vertex_descriptor _vid, AbstractRoadmap _rm)

template<typename AbstractRoadmap >
void	BuildNeighbors (typename SIPPGraph::vertex_descriptor _sippSource, size_t _rmTarget, AbstractRoadmap *_rm)
	Construct the SIPP neighbors for roadmap edge. More...

void	InitializeCostToGo (const std::vector< size_t > _goal)
	Initialize the cost to go from the start point to the goal. More...

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

const std::string &	GetBaseFilename () const

Protected Attributes
Internal State
SIPPGraph *	m_sippGraph {nullptr}
	The graph representing the time-interval extended state space. More...

size_t	m_goalIndex {0}
	Index of the current goal to extend the path to. More...

size_t	m_startVID {SIZE_MAX}
	The start vid in the sipp graph. More...

std::unordered_map< size_t, double >	m_costToGoMap
	Cost-to-go map used for heuristic values. More...

EdgeIntervalMap	m_edgeIntervals
	Set of safe edge intervals. More...

VertexIntervalMap	m_vertexIntervals
	Set of safe vertex intervals. More...

std::unordered_map< size_t, std::unordered_map< size_t, size_t > >	m_waitTimesteps
	Compute wait timesteps at each vertex during the search process. More...

size_t	m_startTime {0}
	The start time of the path. More...

size_t	m_minEndTime {0}
	The minimum end time of the path. More...

bool	m_initialized {false}
	Flag indicating if the object has been initialized. More...

bool	m_minTime {true}
	Flag indiciating if search is minimizing time or distance metric. More...

std::string	m_safeIntervalLabel
	Label of the SI Tool to use. More...

std::string	m_dmLabel
	Distance metric label. More...

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

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

Member Typedef Documentation

◆ EdgeIntervalMap

typedef std::map<std::pair<size_t,size_t>, std::vector<Range<size_t> > > SIPPMethod::EdgeIntervalMap

◆ GroupCfgType

typedef MPBaseObject::GroupCfgType SIPPMethod::GroupCfgType

◆ GroupRoadmapType

typedef MPBaseObject::GroupRoadmapType SIPPMethod::GroupRoadmapType

◆ GroupWeightType

typedef MPBaseObject::GroupWeightType SIPPMethod::GroupWeightType

◆ RoadmapType

typedef MPBaseObject::RoadmapType SIPPMethod::RoadmapType

◆ SIPPGraph

typedef GenericStateGraph<SIPPVertex, SIPPEdge> SIPPMethod::SIPPGraph

◆ VertexIntervalMap

typedef std::map<size_t,std::vector<Range<size_t> > > SIPPMethod::VertexIntervalMap

◆ VIDSet

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

Constructor & Destructor Documentation

◆ SIPPMethod() [1/2]

SIPPMethod::SIPPMethod ( )

◆ SIPPMethod() [2/2]

SIPPMethod::SIPPMethod ( XMLNode & _node )

◆ ~SIPPMethod()

virtual SIPPMethod::~SIPPMethod ( )

virtualdefault

Member Function Documentation

◆ BuildNeighbors()

template<typename AbstractRoadmap >

void SIPPMethod::BuildNeighbors	(	typename SIPPGraph::vertex_descriptor	_sippSource,
		size_t	_rmTarget,
		AbstractRoadmap *	_rm
	)

protected

Construct the SIPP neighbors for roadmap edge.

◆ GeneratePath()

std::pair< std::vector< size_t >, std::vector< size_t > > SIPPMethod::GeneratePath	(	const size_t	_start,
		const std::vector< size_t >	_goals
	)

Generate a path from a start point to a goal point

Parameters

_start	The start coordinate.
_goal	The goal coordinate.

◆ Initialize()

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

◆ InitializeCostToGo()

void SIPPMethod::InitializeCostToGo ( const std::vector< size_t > _goal )

protected

Initialize the cost to go from the start point to the goal.

◆ operator()()

bool SIPPMethod::operator() ( )

overridevirtual

Evaluate a roadmap.

Returns: True if this roadmap meets the evaluation criteria.

Implements MapEvaluatorMethod.

◆ PathWeight()

double SIPPMethod::PathWeight	(	typename SIPPGraph::adj_edge_iterator &	_ei,
		const double	_sourceDistance,
		const double	_targetDistance
	)

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 the distance with a wait time if taking an edge would result in a collision with a dynamic obstacle. If waiting cannot fix, return infinity.

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 including waiting, or infinity if taking this edge would result in a collision with dynamic obstacles.

◆ PerformSubQuery()

bool SIPPMethod::PerformSubQuery	(	const size_t	_start,
		const std::vector< size_t >	_goals
	)

protectedvirtual

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

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 SIPPMethod::Print ( std::ostream & _os ) const

overridevirtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from MPBaseObject.

◆ Reset()

void SIPPMethod::Reset ( )

protectedvirtual

Reset the path and list of undiscovered goals. Also resets wait times and cached safe intervals.

◆ SatisfyConstraints()

bool SIPPMethod::SatisfyConstraints ( Range< size_t > _interval )

Check if the path satisfies all constraints.

◆ SetDMLabel()

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

Set the distance metric to use.

◆ SetEdgeIntervals()

void SIPPMethod::SetEdgeIntervals ( EdgeIntervalMap _edgeIntervals )

Set the edge intervals to use to generate a path.

◆ SetMinEndTime()

void SIPPMethod::SetMinEndTime ( size_t _end )

Set the end time of the query.

◆ SetStartTime()

void SIPPMethod::SetStartTime ( size_t _start )

Set the start time of the query.

◆ SetVertexIntervals()

void SIPPMethod::SetVertexIntervals ( VertexIntervalMap _vertexIntervals )

Set the edge intervals to use to generate a path.

◆ SIPPHeuristic()

double SIPPMethod::SIPPHeuristic	(	const SIPPGraph *	_g,
		typename SIPPGraph::vertex_descriptor	_source,
		typename SIPPGraph::vertex_descriptor	_target
	)

protected

Heuristic function for Safe interval path planning. Calls Dijkstra's from goal node to find shortest path to all nodes in roadmap.

Parameters

_g
_source
_target

Returns

◆ SIPPNeighbors() [1/2]

void SIPPMethod::SIPPNeighbors	(	SIPPGraph *	_g,
		typename SIPPGraph::vertex_descriptor	_vid
	)

protected

Neighbors function for Safe interval path planning.

◆ SIPPNeighbors() [2/2]

template<typename AbstractRoadmap >

void SIPPMethod::SIPPNeighbors	(	SIPPGraph *	_g,
		typename SIPPGraph::vertex_descriptor	_vid,
		AbstractRoadmap *	_rm
	)

protected

Field Documentation

◆ m_costToGoMap

std::unordered_map<size_t,double> SIPPMethod::m_costToGoMap

protected

Cost-to-go map used for heuristic values.

◆ m_dmLabel

std::string SIPPMethod::m_dmLabel

protected

Distance metric label.

◆ m_edgeIntervals

EdgeIntervalMap SIPPMethod::m_edgeIntervals

protected

Set of safe edge intervals.

◆ m_goalIndex

size_t SIPPMethod::m_goalIndex {0}

protected

Index of the current goal to extend the path to.

◆ m_initialized

bool SIPPMethod::m_initialized {false}

protected

Flag indicating if the object has been initialized.

◆ m_minEndTime

size_t SIPPMethod::m_minEndTime {0}

protected

The minimum end time of the path.

◆ m_minTime

bool SIPPMethod::m_minTime {true}

protected

Flag indiciating if search is minimizing time or distance metric.

◆ m_safeIntervalLabel

std::string SIPPMethod::m_safeIntervalLabel

protected

Label of the SI Tool to use.

◆ m_sippGraph

SIPPGraph* SIPPMethod::m_sippGraph {nullptr}

protected

The graph representing the time-interval extended state space.

◆ m_startTime

size_t SIPPMethod::m_startTime {0}

protected

The start time of the path.

◆ m_startVID

size_t SIPPMethod::m_startVID {SIZE_MAX}

protected

The start vid in the sipp graph.

◆ m_vertexIntervals

VertexIntervalMap SIPPMethod::m_vertexIntervals

protected

Set of safe vertex intervals.

◆ m_waitTimesteps

std::unordered_map<size_t,std::unordered_map<size_t,size_t> > SIPPMethod::m_waitTimesteps

protected

Compute wait timesteps at each vertex during the search process.

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

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Member Typedef Documentation

◆ EdgeIntervalMap

◆ GroupCfgType

◆ GroupRoadmapType

◆ GroupWeightType

◆ RoadmapType

◆ SIPPGraph

◆ VertexIntervalMap

◆ VIDSet

Constructor & Destructor Documentation

◆ SIPPMethod() [1/2]

◆ SIPPMethod() [2/2]

◆ ~SIPPMethod()

Member Function Documentation

◆ BuildNeighbors()

◆ GeneratePath()

◆ Initialize()

◆ InitializeCostToGo()

◆ operator()()

◆ PathWeight()

◆ PerformSubQuery()

◆ Print()

◆ Reset()

◆ SatisfyConstraints()

◆ SetDMLabel()

◆ SetEdgeIntervals()

◆ SetMinEndTime()

◆ SetStartTime()

◆ SetVertexIntervals()

◆ SIPPHeuristic()

◆ SIPPNeighbors() [1/2]

◆ SIPPNeighbors() [2/2]

Field Documentation

◆ m_costToGoMap

◆ m_dmLabel

◆ m_edgeIntervals

◆ m_goalIndex

◆ m_initialized

◆ m_minEndTime

◆ m_minTime

◆ m_safeIntervalLabel

◆ m_sippGraph

◆ m_startTime

◆ m_startVID

◆ m_vertexIntervals

◆ m_waitTimesteps