GoalTracker.h

Go to the documentation of this file.

#ifndef PMPL_GOAL_TRACKER_H_
#define PMPL_GOAL_TRACKER_H_
 
#include "MPLibrary/MPBaseObject.h"
#include "MPProblem/Constraints/Constraint.h"
#include "MPProblem/GroupTask.h"
#include "MPProblem/MPTask.h"
#include "MPProblem/RobotGroup/RobotGroup.h"
 
#include <atomic>
#include <map>
#include <unordered_set>
#include <vector>
 
 
template <typename Roadmap, typename Task>
class GoalMap {
 
  public:
 
 
    typedef typename Roadmap::VI  VI;
    typedef typename Roadmap::VID VID;
 
 
    typedef std::unordered_set<VID> VIDSet;
 
 
    GoalMap(Roadmap* const _r, const Task* const _t);
 
    ~GoalMap();
 
 
    const VIDSet& GetStartVIDs() const noexcept;
 
    const VIDSet& GetGoalVIDs(const size_t _index) const noexcept;
 
    size_t UnreachedGoalCount() const noexcept;
 
    std::vector<size_t> UnreachedGoalIndexes() const noexcept;
 
 
    void InstallHooks() noexcept;
 
    void RemoveHooks() noexcept;
 
 
  private:
 
 
    void TrackCfg(const VI _vertex);
 
    void UntrackCfg(const VI _vertex);
 
    std::string GetHookLabel() const noexcept;
 
 
    static std::atomic<size_t> s_index; 
 
    const size_t m_index;      
    Roadmap* const m_roadmap;  
    const Task* const m_task;  
 
    VIDSet m_startVIDs;
 
    std::vector<VIDSet> m_goalVIDs;
 
};
 
/*------------------------------- Construction -------------------------------*/
 
// Initalize the global index.
template <typename Roadmap, typename Task>
std::atomic<size_t> GoalMap<Roadmap, Task>::s_index(0);
 
 
template <typename Roadmap, typename Task>
GoalMap<Roadmap, Task>::
GoalMap(Roadmap* const _roadmap, const Task* const _task)
    : m_index(++s_index), m_roadmap(_roadmap), m_task(_task) {
  // If either roadmap or task is null, we cannot build this object.
  if(!_roadmap)
    throw RunTimeException(WHERE) << "A non-null roadmap pointer is required.";
  if(!_task)
    throw RunTimeException(WHERE) << "A non-null task pointer is required.";
 
  // If we have no start or goal constraints, then this object is not needed and
  // will do nothing.
  if(m_task->Empty())
    return;
 
  // Initialize the goalVIDs structure with the correct number of constraints.
  m_goalVIDs.resize(m_task->GetNumGoals());
 
  // Evaluate any existing configurations.
  for(auto vi = m_roadmap->begin(); vi != m_roadmap->end(); ++vi)
    TrackCfg(vi);
}
 
 
template <typename Roadmap, typename Task>
GoalMap<Roadmap, Task>::
~GoalMap() {
}
 
/*-------------------------------- Interface ---------------------------------*/
 
template <typename Roadmap, typename Task>
const typename GoalMap<Roadmap, Task>::VIDSet&
GoalMap<Roadmap, Task>::
GetStartVIDs() const noexcept {
  return m_startVIDs;
}
 
 
template <typename Roadmap, typename Task>
const typename GoalMap<Roadmap, Task>::VIDSet&
GoalMap<Roadmap, Task>::
GetGoalVIDs(const size_t _index) const noexcept {
  if(_index > m_goalVIDs.size())
    throw RunTimeException(WHERE) << "Requested goal VIDs for constraint "
                                  << _index << ", but only "
                                  << m_goalVIDs.size() << " constraints are "
                                  << "present in task '" << m_task->GetLabel()
                                  << "'.";
  return m_goalVIDs[_index];
}
 
 
template <typename Roadmap, typename Task>
size_t
GoalMap<Roadmap, Task>::
UnreachedGoalCount() const noexcept {
  size_t count = 0;
 
  // Add up the number of goal constraints that have no satisfying vids.
  for(const auto& vids : m_goalVIDs)
    count += vids.empty();
 
  return count;
}
 
 
template <typename Roadmap, typename Task>
std::vector<size_t>
GoalMap<Roadmap, Task>::
UnreachedGoalIndexes() const noexcept {
  std::vector<size_t> indexes;
 
  // Add up the number of goal constraints that have no satisfying vids.
  for(size_t i = 0; i < m_goalVIDs.size(); ++i)
    if(m_goalVIDs[i].empty())
      indexes.push_back(i);
 
  return indexes;
}
 
/*---------------------------------- Hooks -----------------------------------*/
 
template <typename Roadmap, typename Task>
void
GoalMap<Roadmap, Task>::
InstallHooks() noexcept {
  using HT = typename Roadmap::HookType;
 
  const std::string& label = this->GetHookLabel();
 
  m_roadmap->InstallHook(HT::AddVertex, label,
      [this](const VI _vi){this->TrackCfg(_vi);});
 
  m_roadmap->InstallHook(HT::DeleteVertex, label,
      [this](const VI _vi){this->UntrackCfg(_vi);});
}
 
 
template <typename Roadmap, typename Task>
void
GoalMap<Roadmap, Task>::
RemoveHooks() noexcept {
  using HT = typename Roadmap::HookType;
 
  const std::string& label = this->GetHookLabel();
 
  if(m_roadmap->IsHook(HT::AddVertex, label))
    m_roadmap->RemoveHook(HT::AddVertex, label);
 
  if(m_roadmap->IsHook(HT::DeleteVertex, label))
    m_roadmap->RemoveHook(HT::DeleteVertex, label);
}
 
/*--------------------------------- Helpers ----------------------------------*/
 
template <typename Roadmap, typename Task>
void
GoalMap<Roadmap, Task>::
TrackCfg(const VI _vertex) {
  // Get the VID and cfg info.
  const VID vid   = _vertex->descriptor();
  const auto& cfg = _vertex->property();
 
  // Check for satisfying the start constraint.
  if(m_task->EvaluateStartConstraints(cfg))
    m_startVIDs.insert(vid);
 
  // Check for satisfying each goal constraint.
  const size_t numGoals = m_task->GetNumGoals();
  for(size_t i = 0; i < numGoals; ++i)
    if(m_task->EvaluateGoalConstraints(cfg, i))
      m_goalVIDs[i].insert(vid);
}
 
 
template <typename Roadmap, typename Task>
void
GoalMap<Roadmap, Task>::
UntrackCfg(const VI _vertex) {
  // Get the VID info.
  const VID vid = _vertex->descriptor();
 
  // Untrack satisfaction of the start constraint.
  if(m_startVIDs.count(vid))
    m_startVIDs.erase(vid);
 
  // Untrack satisfaction of the goal constraints.
  for(auto& vids : m_goalVIDs)
    if(vids.count(vid))
      vids.erase(vid);
}
 
 
template <typename Roadmap, typename Task>
std::string
GoalMap<Roadmap, Task>::
GetHookLabel() const noexcept {
  return "GoalMap" + std::to_string(m_index);
}
 
/*----------------------------------------------------------------------------*/
 
 
class GoalTrackerType final : MPBaseObject {
 
  public:
 
 
    typedef typename MPBaseObject::RoadmapType       RoadmapType;
    typedef typename MPBaseObject::GroupRoadmapType  GroupRoadmapType;
    typedef typename RoadmapType::VID                VID;
 
 
    typedef std::unordered_set<VID> VIDSet;
 
 
    GoalTrackerType(MPLibrary* const _library);
 
    ~GoalTrackerType();
 
 
    template <typename Roadmap, typename Task>
    const VIDSet& GetStartVIDs(Roadmap* const _roadmap, const Task* const _task)
        const;
 
    const VIDSet& GetStartVIDs() const;
 
    template <typename Roadmap, typename Task>
    const VIDSet& GetGoalVIDs(Roadmap* const _roadmap, const Task* const _task,
        const size_t _index) const;
 
    const VIDSet& GetGoalVIDs(const size_t _index) const;
 
    template <typename Roadmap, typename Task>
    size_t UnreachedGoalCount(Roadmap* const _roadmap, const Task* const _task)
        const;
 
    size_t UnreachedGoalCount() const;
 
    template <typename Roadmap, typename Task>
    std::vector<size_t> UnreachedGoalIndexes(Roadmap* const _roadmap,
        const Task* const _task) const;
 
    std::vector<size_t> UnreachedGoalIndexes() const;
 
    void AddMap(RoadmapType* const _roadmap, const MPTask* const _task);
 
    void AddMap(GroupRoadmapType* const _roadmap, const GroupTask* const _task);
 
    bool IsMap(RoadmapType* const _roadmap, const MPTask* const _task) const;
 
    bool IsMap(GroupRoadmapType* const _roadmap, const GroupTask* const _task)
        const;
 
    void Clear();
 
 
  private:
 
 
    typedef std::pair<RoadmapType*, const MPTask*>         IndividualKey;
    typedef std::pair<GroupRoadmapType*, const GroupTask*> GroupKey;
 
    typedef GoalMap<RoadmapType, MPTask>                   IndividualMap;
    typedef GoalMap<GroupRoadmapType, GroupTask>           GroupMap;
 
 
    const IndividualMap& GetMap(RoadmapType* const _roadmap,
        const MPTask* const _task) const;
 
    const GroupMap& GetMap(GroupRoadmapType* const _roadmap,
        const GroupTask* const _task) const;
 
 
    std::map<IndividualKey, IndividualMap> m_individualMaps;
    std::map<GroupKey, GroupMap> m_groupMaps;
 
 
};
 
#endif