PPL API Reference
Overview Core Algorithm Abstractions Utilities Parallel Methods
Modules Class Hierarchy Classes Functions
Todo List Bug List Dead Code
List of all members | Protected Member Functions | Protected Attributes
SIPPMethod< MPTraits > Class Template Reference

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::vector< size_t > GeneratePath (const size_t _start, const size_t _goal)
 
void SetDMLabel (const std::string &_dmLabel)
 Set the distance metric to use.
 
void SetStartTime (double _start)
 Set the start time of the query.
 
void SetEndTime (double _end)
 Set the end time of the query.
 
virtual void SetEdgeIntervals (EdgeIntervals _edgeIntervals) override
 Set the edge intervals to use to generate a path. More...
 
virtual void SetMinEndtime (double _minEndtime) override
 Set the minimum end time of a path. More...
 
bool SatisfyConstraints ()
 Check if the path satisfies all constraints.
 
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.
 
MPLibraryGetMPLibrary () 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::MPToolsGetMPTools () const noexcept
 Get the MPTools container from the owning MPLibrary.
 
Problem Accessors
MPProblemGetMPProblem () const noexcept
 Get the library's current MPProblem.
 
EnvironmentGetEnvironment () const noexcept
 Get the current environment.
 
MPTaskGetTask () const noexcept
 Get the current task.
 
GroupTaskGetGroupTask () const noexcept
 Get the current group task.
 
Solution Accessors
MPSolutionGetMPSolution () const noexcept
 
RoadmapTypeGetRoadmap (Robot *const _r=nullptr) const noexcept
 Get the current free-space roadmap.
 
GroupRoadmapTypeGetGroupRoadmap (RobotGroup *const _g=nullptr) const noexcept
 Get the current free-space group roadmap.
 
RoadmapTypeGetBlockRoadmap (Robot *const _r=nullptr) const noexcept
 Get the current obstacle-space roadmap.
 
PathGetPath (Robot *const _r=nullptr) const noexcept
 Get the current best path.
 
GroupPathGetGroupPath (RobotGroup *const _g=nullptr) const noexcept
 Get the current best group path.
 
StatClassGetStatClass () const noexcept
 Get the current StatClass.
 
LocalObstacleMapGetLocalObstacleMap () const noexcept
 Get the local obstacle map.
 
GoalTrackerGetGoalTracker () const noexcept
 Get the goal tracker.
 

Public Types

Motion Planning Types
typedef MPTraits::GoalTracker GoalTracker
 
typedef GoalTracker::VIDSet VIDSet
 
typedef std::unordered_map< size_t, std::unordered_map< size_t, std::vector< Range< double > > > > EdgeIntervals
 
Classes and Structures
typedef RoadmapGraph< Cfg, DefaultWeight< Cfg > > RoadmapGraphType
 
typedef RoadmapGraph< State, DefaultWeight< Cfg > > SIPPGraph
 
Local Types
typedef MPTraits::RoadmapType RoadmapType
 
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
virtual void Reset (RoadmapGraphType *const _r)
 
void computeIntervals ()
 
virtual bool PerformSubQuery (const size_t _start, const size_t _goal)
 
virtual double DynamicPathWeight (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 _vd)
 Neighbors function for Safe interval path planning.
 
void InitializeCostToGo (size_t _goal)
 Initialize the cost to go from the start point to the goal.
 
Range< double > IntervalOverlaps (std::vector< Range< double > > _intervals, Range< double > _interval)
 
Range< double > IntervalContains (std::vector< Range< double > > _intervals, double timestep)
 Check if any of the safe intervals in _intervals contains timestep.
 
std::vector< Range< double > > OverlappingIntervals (std::vector< Range< double > > _intervalsA, std::vector< Range< double > > _intervalsB)
 Calculates the overlapping safe intervals between _intervalsA and _intervalsB.
 
void cleanup ()
 

Protected Attributes

bool m_debug
 Print debug info?
 
Internal State
RoadmapGraphTypem_roadmap {nullptr}
 
SIPPGraphm_sippGraph {nullptr}
 
MPTaskm_task {nullptr}
 
std::string m_safeIntervalLabel
 
std::string m_dmLabel
 
std::unordered_map< size_t, double > m_costToGoMap
 
std::unordered_map< size_t, std::vector< Range< double > > > m_roadmap_intervals
 
std::unordered_map< size_t, std::unordered_set< typename SIPPGraph::vertex_descriptor > > m_sipp_mappings
 
std::unordered_map< typename RoadmapGraphType::vertex_descriptor, bool > m_changed
 
std::unordered_map< size_t, unordered_map< size_t, std::vector< Range< double > > > > m_edgeIntervals
 
std::vector< size_t > m_wait_timesteps
 
std::unordered_map< size_t, std::unordered_map< size_t, double > > m_transitionWait
 
size_t m_goalIndex {0}
 
double m_startTime {0}
 
double m_endTime {0}
 
double m_minEndtime {0}
 
double m_pathCost {0}
 
size_t m_currentGoalVID {0}
 
bool m_initialized {false}
 
bool m_invervalsSet {false}
 
bool m_sippSet {false}
 
Internal State
std::vector< size_t > m_activeRobots
 

Detailed Description

template<typename MPTraits>
class SIPPMethod< MPTraits >

Map evaluator for Safe Interval Path Planning (SIPP) Utilizes MPTools/SafeIntervalTools to help calculate Safe intervals for vertices and edges

Member Function Documentation

◆ Print()

template<typename MPTraits >
void SIPPMethod< MPTraits >::Print ( std::ostream &  _os) const
overridevirtual

Print internal state of this object.

Parameters
_osThe std::ostream to print to.

Reimplemented from MPBaseObject< MPTraits >.

◆ Initialize()

template<typename MPTraits >
void SIPPMethod< 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 MPBaseObject< MPTraits >.

◆ operator()()

template<typename MPTraits >
bool SIPPMethod< MPTraits >::operator() ( )
overridevirtual

Evaluate a roadmap.

Returns
True if this roadmap meets the evaluation criteria.

Implements MapEvaluatorMethod< MPTraits >.

◆ GeneratePath()

template<typename MPTraits >
std::vector< size_t > SIPPMethod< MPTraits >::GeneratePath ( const size_t  _start,
const size_t  _goal 
)

Generate a path from a start point to a goal point

Parameters
_startThe start coordinate.
_goalThe goal coordinate.

◆ SetEdgeIntervals()

template<typename MPTraits >
void SIPPMethod< MPTraits >::SetEdgeIntervals ( EdgeIntervals  _edgeIntervals)
overridevirtual

Set the edge intervals to use to generate a path.

Reimplemented from MapEvaluatorMethod< MPTraits >.

◆ SetMinEndtime()

template<typename MPTraits >
void SIPPMethod< MPTraits >::SetMinEndtime ( double  _minEndtime)
overridevirtual

Set the minimum end time of a path.

Reimplemented from MapEvaluatorMethod< MPTraits >.

◆ Reset()

template<typename MPTraits >
void SIPPMethod< MPTraits >::Reset ( RoadmapGraphType *const  _r)
protectedvirtual

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

◆ computeIntervals()

template<typename MPTraits >
void SIPPMethod< MPTraits >::computeIntervals
protected

TODO: Remove this Calculates the safe intervals for each vertex in the roadmap

◆ PerformSubQuery()

template<typename MPTraits >
bool SIPPMethod< MPTraits >::PerformSubQuery ( const size_t  _start,
const size_t  _goal 
)
protectedvirtual

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

Parameters
_startThe start VID to use.
_goalsThe goal VIDs to use.
Returns
True if a path from _start to one of _goals was generated.

◆ DynamicPathWeight()

template<typename MPTraits >
double SIPPMethod< MPTraits >::DynamicPathWeight ( 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
_eiAn iterator to the edge we are checking.
_sourceDistanceThe shortest time to the source node.
_targetDistanceThe 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.

◆ SIPPHeuristic()

template<typename MPTraits >
double SIPPMethod< MPTraits >::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

◆ IntervalOverlaps()

template<typename MPTraits >
Range< double > SIPPMethod< MPTraits >::IntervalOverlaps ( std::vector< Range< double > >  _intervals,
Range< double >  _interval 
)
protected

Calculate the overlap between a safe interval of one configuration and the safe intervals of another configuration

◆ SetName()

template<typename MPTraits >
void MPBaseObject< MPTraits >::SetName ( const std::string &  _s)
protectedinherited
Parameters
_sClass 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: