SIPPMethod.h

Go to the documentation of this file.

#ifndef PPL_SIPP_METHOD_H_
#define PPL_SIPP_METHOD_H_
 
#include "MapEvaluatorMethod.h"
 
#include "ConfigurationSpace/Path.h"
#include "ConfigurationSpace/GenericStateGraph.h"
#include "ConfigurationSpace/GroupLocalPlan.h"
#include "ConfigurationSpace/GroupRoadmap.h"
 
#include "Geometry/Boundaries/Range.h"
 
#include "MPLibrary/LocalPlanners/LPOutput.h"
#include "MPLibrary/LocalPlanners/StraightLine.h"
#include "MPLibrary/MPTools/SafeIntervalTool.h"
 
#include "MPProblem/GroupTask.h"
#include "MPProblem/MPProblem.h"
#include "MPProblem/MPTask.h"
#include "MPProblem/Robot/Robot.h"
#include "MPProblem/RobotGroup/RobotGroup.h"
 
struct SIPPVertex {
  size_t vid;
  Range<size_t> interval;
 
  bool operator==(const SIPPVertex& _other) const {
    return vid      == _other.vid &&
    interval == _other.interval;
  }
};
 
struct SIPPEdge {
  size_t source;
  size_t target;
  Range<double> interval;
};
 
class SIPPMethod : public MapEvaluatorMethod {
 
  public:
 
 
    typedef std::unordered_set<size_t>              VIDSet;
    typedef typename MPBaseObject::RoadmapType      RoadmapType;
    typedef typename MPBaseObject::GroupRoadmapType GroupRoadmapType;
    typedef typename MPBaseObject::GroupCfgType     GroupCfgType;
    typedef typename 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;
 
 
    SIPPMethod();
 
    SIPPMethod(XMLNode& _node);
 
    virtual ~SIPPMethod() = default;
 
 
    virtual void Print(std::ostream& _os) const override;
 
    virtual void Initialize() override;
 
 
    virtual bool operator()() override;
 
 
    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);
 
    void SetStartTime(size_t _start);
 
    void SetMinEndTime(size_t _end);
 
    void SetEdgeIntervals(EdgeIntervalMap _edgeIntervals);
 
    void SetVertexIntervals(VertexIntervalMap _vertexIntervals);
 
    bool SatisfyConstraints(Range<size_t> _interval);
 
 
  protected:
 
 
    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);
 
    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);
 
    void InitializeCostToGo(const std::vector<size_t> _goal);
 
 
    SIPPGraph* m_sippGraph{nullptr}; 
 
    size_t m_goalIndex{0};      
    size_t m_startVID{SIZE_MAX}; 
 
    std::unordered_map<size_t,double> m_costToGoMap;
 
    EdgeIntervalMap m_edgeIntervals; 
    VertexIntervalMap m_vertexIntervals; 
 
    std::unordered_map<size_t,std::unordered_map<size_t,size_t>> m_waitTimesteps;
 
    size_t m_startTime{0}; 
    size_t m_minEndTime{0}; 
 
    bool m_initialized{false}; 
    bool m_minTime{true}; 
 
    // James: We no longer use the SI Tool to compute intervals.
    // More accurately, we no longer compute intervals within this class and instead 
    // set them externally. That encapsulation is fine, but we should remove this.
    std::string m_safeIntervalLabel; 
    std::string m_dmLabel;           
 
};
 
template <typename AbstractRoadmap>
void
SIPPMethod::
SIPPNeighbors(SIPPGraph* _g,
    typename SIPPGraph::vertex_descriptor _vid,
    AbstractRoadmap* _rm) {
 
  auto vertex = _g->GetVertex(_vid);
  auto vid = vertex.vid;
 
  // Get vertex iterator for vid
  auto vit = _rm->find_vertex(vid);
 
  // Check that vertex is contained in roadmap
  if(vit == _rm->end())
    throw RunTimeException(WHERE) << vid << " does not existing in roadmap.";
 
  // Build SIPP Neighbors for each roadmap neighbor
  for(auto eit = vit->begin(); eit != vit->end(); eit++) {
    BuildNeighbors(_vid,eit->target(),_rm);
  }
}
 
 
template <typename AbstractRoadmap>
void
SIPPMethod::
BuildNeighbors(typename SIPPGraph::vertex_descriptor _sippSource, size_t _rmTarget,
               AbstractRoadmap* _rm) {
 
  const auto vertex = m_sippGraph->GetVertex(_sippSource);
  const size_t rmSource = vertex.vid;
 
  // Find candidate intervals for target vertex
  std::vector<Range<size_t>> endIntervals;
  if(!m_vertexIntervals.empty()) { 
    endIntervals = m_vertexIntervals[_rmTarget];
  }
  else {
    endIntervals.push_back(Range<size_t>(0,SIZE_MAX));
  }
 
  for(auto& inter : endIntervals) {
 
    // Add new sipp vertex and edge to graph
    SIPPVertex targetVertex;
    targetVertex.vid = _rmTarget;
    targetVertex.interval = inter;
 
    auto targetVID = m_sippGraph->AddVertex(targetVertex);
    
    SIPPEdge newEdge;
    newEdge.source = rmSource;
    newEdge.target = _rmTarget;
 
    m_sippGraph->AddEdge(_sippSource,targetVID,newEdge);
  }
 
}
 
std::istream& operator>>(std::istream& _is, SIPPVertex& _vertex);
 
std::ostream& operator<<(std::ostream& _os, const SIPPVertex& _vertex);
 
std::istream& operator>>(std::istream& _is, SIPPEdge& _edge);
 
std::ostream& operator<<(std::ostream& _os, const SIPPEdge& _edge);
 
#endif