GroupPath.h

Go to the documentation of this file.

#ifndef PMPL_GROUP_PATH_H_
#define PMPL_GROUP_PATH_H_
 
#include "ConfigurationSpace/GenericStateGraph.h"
#include "ConfigurationSpace/Cfg.h"
#include "ConfigurationSpace/Weight.h"
#include "ConfigurationSpace/GroupRoadmap.h"
#include "ConfigurationSpace/GroupCfg.h"
#include "ConfigurationSpace/GroupLocalPlan.h"
// #include "MPLibrary/MPLibrary.h"
#include "Utilities/PMPLExceptions.h"
 
#include <algorithm>
#include <vector>
 
 
class GroupPath final {
 
  public:
 
 
    typedef GenericStateGraph<Cfg, DefaultWeight<Cfg>> RoadmapType;
    typedef GroupCfg<RoadmapType> GroupCfgType;
    typedef GroupRoadmap<GroupCfgType, GroupLocalPlan<RoadmapType>> GroupRoadmapType;
    typedef typename GroupRoadmapType::VID VID;
 
 
    GroupPath(GroupRoadmapType* const _r = nullptr);
 
 
    GroupRoadmapType* GetRoadmap() const noexcept;
 
    size_t Size() const noexcept;
 
    bool Empty() const noexcept;
 
    double Length() const;
 
    size_t TimeSteps() const;
 
    const std::vector<VID>& VIDs() const noexcept;
 
        const std::pair<std::vector<VID>,std::vector<size_t>> VIDsWaiting() const noexcept;
 
    const std::vector<GroupCfgType>& Cfgs() const;
 
    template <typename MPLibrary>
    const std::vector<GroupCfgType> FullCfgs(MPLibrary* const _lib) const;
 
    template <typename MPLibrary>
    const std::vector<GroupCfgType> FullCfgsWithWait(MPLibrary* const _lib) const;
 
    GroupPath& operator+=(const GroupPath& _p);
 
    GroupPath operator+(const GroupPath& _p) const;
 
    GroupPath& operator+=(const std::vector<VID>& _vids);
 
    GroupPath operator+(const std::vector<VID>& _vids) const;
 
    GroupPath& operator=(const GroupPath& _p);
 
    void Clear();
 
    void FlushCache();
 
    void SetWaitTimes(std::vector<size_t> _waitTimes);
  
    std::vector<size_t> GetWaitTimes();
 
    std::pair<size_t,size_t> GetEdgeAtTimestep(size_t _timestep);
 
 
  private:
 
 
    void AssertSameMap(const GroupPath& _p) const;
 
 
    GroupRoadmapType* const m_roadmap;     
    std::vector<VID> m_vids;               
    std::vector<size_t> m_waitingTimesteps; 
        size_t m_finalWaitTimeSteps{0}; 
 
    mutable std::vector<GroupCfgType> m_cfgs;  
    mutable bool m_cfgsCached{false};      
 
    mutable double m_length{0};            
    mutable bool m_lengthCached{false};    
 
    mutable double m_timesteps{0};            
    mutable bool m_timestepsCached{false};    
};
 
 
template <typename MPLibrary>
const std::vector<typename GroupPath::GroupCfgType>
GroupPath::
FullCfgs(MPLibrary* const _lib) const {
  if(m_vids.empty())
    return std::vector<GroupCfgType>();
 
  // Insert the first vertex.
  std::vector<GroupCfgType> out = {m_roadmap->GetVertex(m_vids.front())};
 
  for(auto it = m_vids.begin(); it + 1 < m_vids.end(); ++it) {
    const VID source = *it,
              target = *(it + 1);
    // const auto& edge = m_roadmap->GetEdge(source, target);
 
    // Insert intermediates between vertices. For assembly planning (skip edge):
    // don't reconstruct the edge when it's for a part that has been placed off
    // to the side, just use the two cfgs. This edge will just be (start, end).
    // if(!edge.SkipEdge()) {
      auto e = m_roadmap->GetEdge(source,target);
      auto edge = !e.GetIntermediates().empty() ? e.GetIntermediates()
                                              : _lib->ReconstructEdge(m_roadmap, source, target);
 
      if(!edge.empty()) {
        // Only grab the intermediate cfgs.
        auto startIter = edge.begin();
 
        auto endIter = edge.end();
 
        out.insert(out.end(), startIter, endIter);
      }
    // }
  
    // Insert the next vertex.
    out.push_back(m_roadmap->GetVertex(target));
  }
  return out;
}
 
 
template <typename MPLibrary>
const std::vector<typename GroupPath::GroupCfgType>
GroupPath::
FullCfgsWithWait(MPLibrary* const _lib) const {
  if(m_vids.empty())
    return std::vector<GroupCfgType>();
 
  if(m_waitingTimesteps.empty())
    return FullCfgs(_lib);
 
  // Insert the first vertex.
  size_t vid = m_vids.front();
  GroupCfgType vertex = m_roadmap->GetVertex(vid);
  std::vector<GroupCfgType> out = {vertex};
 
  // Insert first vertex however many timesteps it waits
  //for(size_t i = 0; i < m_waitingTimesteps[0]; ++i) {
  for(size_t i = 0; i < m_waitingTimesteps[0]; ++i) {
    out.push_back(vertex);
  }
 
  auto cnt = 1;
  for(auto it = m_vids.begin(); it + 1 < m_vids.end(); ++it) {
    // Insert intermediates between vertices.
 
    //std::vector<GroupCfgType> edge = _lib->ReconstructEdge(m_roadmap, *it, *(it+1));
    auto e = m_roadmap->GetEdge(*it,*(it+1));
    auto edge = !e.GetIntermediates().empty() ? e.GetIntermediates()
                                              : _lib->ReconstructEdge(m_roadmap,*it,*(it+1));
    out.insert(out.end(), edge.begin(), edge.end());
 
    // Insert the next vertex.
    vid = *(it+1);
    vertex = m_roadmap->GetVertex(vid);
    out.push_back(vertex);
    for(size_t i = 0; i < m_waitingTimesteps[cnt]; ++i) {
      out.push_back(vertex);
    }
 
    cnt++;
  }
 
  auto last = out.back();
  for(size_t i = 0; i < m_finalWaitTimeSteps; i++) {
    out.push_back(last);
  }
  return out;
}
 
#endif