CBSQuery.h

Go to the documentation of this file.

#ifndef PPL_CBS_QUERY_H_
#define PPL_CBS_QUERY_H_
 
#include "MapEvaluatorMethod.h"
#include "SIPPMethod.h"
#include "Utilities/CBS.h"
 
class CBSQuery : public MapEvaluatorMethod {
 
  public:
 
 
    typedef typename MPBaseObject::RoadmapType   RoadmapType;
    typedef typename RoadmapType::VID        VID;
    typedef typename RoadmapType::EdgeID     EdgeID;
 
 
  private:
 
 
    // Edge <Source, Target>, Time Interval <Start, End> 
    typedef std::pair<std::pair<size_t,size_t>,Range<size_t>> Constraint;
    typedef CBSNode<Robot,Constraint,Path>    Node;
 
    typedef std::map<size_t,std::vector<Range<size_t>>> VertexIntervals;
    typedef std::map<std::pair<size_t,size_t>,std::vector<Range<size_t>>> EdgeIntervals;
 
 
  public:
 
 
    CBSQuery();
 
    CBSQuery(XMLNode& _node);
 
    virtual ~CBSQuery() = default;
 
 
    virtual void Initialize() override;
 
 
    virtual bool operator()() override;
 
 
  protected:
 
 
    Path* SolveIndividualTask(Robot* const _robot, Node& _node);
 
    std::vector<std::pair<Robot*,Constraint>> ValidationFunction(Node& _node);
 
    std::vector<Node> SplitNodeFunction(Node& _node, 
          std::vector<std::pair<Robot*, Constraint>> _constraints,
          CBSLowLevelPlanner<Robot, Constraint, Path>& _lowlevel,
          CBSCostFunction<Robot, Constraint, Path>& _cost);
 
    double CostFunction(Node& _node);
 
 
    void ComputeIntervals(Robot* _robot, const Node& _node);
 
    std::vector<Range<size_t>> ConstructSafeIntervals(std::vector<Range<size_t>>& _unsafeIntervals);
 
    bool OverlappingIntervals(Range<size_t> _interval1, Range<size_t> _interval2);
 
    Range<double> MergeIntervals(Range<double> _interval1, Range<double> _interval2);
 
 
    std::vector<Robot*> m_robots; 
 
    std::string m_queryLabel;  
    std::string m_vcLabel;     
    std::string m_costLabel = "SOC"; 
 
    size_t m_nodeLimit{std::numeric_limits<size_t>::max()}; 
 
    std::unordered_map<Robot*, MPTask*> m_taskMap; 
 
    VertexIntervals m_vertexIntervals;  
    EdgeIntervals m_edgeIntervals;      
 
 
};
 
#endif