CompositeState.h

Go to the documentation of this file.

#ifndef PPL_COMPOSITE_STATE_H_
#define PPL_COMPOSITE_STATE_H_
 
#include "ConfigurationSpace/CompositeGraph.h"
#include "ConfigurationSpace/CompositeEdge.h"
 
#include "MPProblem/RobotGroup/RobotGroup.h"
#include "nonstd.h"
 
#include <cstddef>
#include <iostream>
#include <vector>
 
#include "Vector.h"
 
class Robot;
class RobotGroup;
template <typename GraphType> class CompositeEdge;
 
 
template <typename GraphType>
class CompositeState {
 
  public:
 
 
    typedef size_t           VID;      
    typedef std::vector<VID> VIDSet;   
    typedef typename GraphType::CfgType CfgType; 
 
    typedef CompositeEdge<GraphType>                          CompositeEdgeType;
    typedef CompositeGraph<CompositeState, CompositeEdgeType> GroupGraphType;
 
    typedef GraphType IndividualGraph;
 
 
    explicit CompositeState(GroupGraphType* const _groupGraph = nullptr);
 
    explicit CompositeState(RobotGroup* const _group);
 
 
    virtual bool operator==(const CompositeState& _other) const noexcept;
 
    virtual bool operator!=(const CompositeState& _other) const noexcept;
 
 
    virtual size_t GetNumRobots() const noexcept;
 
    virtual const std::vector<Robot*>& GetRobots() const noexcept;
 
    virtual Robot* GetRobot(const size_t _index) const;
 
 
    virtual GroupGraphType* GetGroupGraph() const noexcept;
 
    virtual void SetGroupGraph(GroupGraphType* _newGraph);
 
    virtual VID GetVID(const size_t _index) const noexcept;
 
    virtual VID GetVID(Robot* const _robot) const;
 
 
    virtual void SetRobotCfg(Robot* const _robot, const VID _vid);
 
    virtual void SetRobotCfg(const size_t _index, const VID _vid);
 
    virtual void SetRobotCfg(Robot* const _robot, CfgType&& _cfg);
 
    virtual void SetRobotCfg(const size_t _index, CfgType&& _cfg);
 
    virtual CfgType& GetRobotCfg(Robot* const _robot);
 
    virtual CfgType& GetRobotCfg(const size_t _index);
 
    virtual const CfgType& GetRobotCfg(Robot* const _robot) const;
 
    virtual const CfgType& GetRobotCfg(const size_t _index) const;
 
    void ClearLocalCfgs();
 
 
  protected:
 
 
    virtual void VerifyIndex(const size_t _robotIndex) const noexcept;
 
    virtual bool IsLocalCfg(const size_t _robotIndex) const noexcept;
 
    virtual void InitializeLocalCfgs() noexcept;
 
 
    GroupGraphType* m_groupMap{nullptr};  
 
    RobotGroup* m_group{nullptr}; 
 
    VIDSet m_vids;   
 
    std::vector<CfgType> m_localCfgs; 
 
 
};
 
template <typename GraphType>
std::ostream& operator<<(std::ostream&, const CompositeState<GraphType>&);
 
/*------------------------------- Construction -------------------------------*/
 
template <typename GraphType>
CompositeState<GraphType>::
CompositeState(GroupGraphType* const _groupGraph) : m_groupMap(_groupGraph) {
 
  // If no group graph was given, this is a placeholder object.
  if(!m_groupMap)
    return;
 
  // Set the group to the one given in the composite graph.
  m_group = m_groupMap->GetGroup();
 
  // Set the VID list to all invalid.
  m_vids.resize(GetNumRobots(), INVALID_VID);
 
  // Initialize local configurations.
  InitializeLocalCfgs();
}
 
 
template <typename GraphType>
CompositeState<GraphType>::
CompositeState(RobotGroup* const _group) : m_group(_group) {
  // Set the VID list to all invalid.
  m_vids.resize(GetNumRobots(), INVALID_VID);
 
  // Initialize local configurations.
  InitializeLocalCfgs();
}
 
 
/*--------------------------------- Equality ---------------------------------*/
 
template <typename GraphType>
bool
CompositeState<GraphType>::
operator==(const CompositeState<GraphType>& _other) const noexcept {
  // If _other is for another group, these are not the same.
  if(m_group != _other.m_group)
    return false;
 
  // If _other is from another graph, these are not the same.
  if(m_groupMap != _other.m_groupMap)
    return false;
 
  // Else, compare VIDs if both are valid, or by-value other wise.
  for(size_t i = 0; i < m_vids.size(); ++i) {
    const VID thisVID  = m_vids[i],
              otherVID = _other.m_vids[i];
 
    if(thisVID != INVALID_VID and otherVID != INVALID_VID) {
      if(thisVID != otherVID)
        return false;
    }
    else if(GetRobotCfg(i) != _other.GetRobotCfg(i))
      return false;
  }
 
  return true;
}
 
 
template <typename GraphType>
bool
CompositeState<GraphType>::
operator!=(const CompositeState<GraphType>& _other) const noexcept {
  return !(*this == _other);
}
 
/*---------------------------------- Robots ----------------------------------*/
 
template <typename GraphType>
size_t
CompositeState<GraphType>::
GetNumRobots() const noexcept {
  return m_group ? m_group->Size() : 0;
}
 
 
template <typename GraphType>
const std::vector<Robot*>&
CompositeState<GraphType>::
GetRobots() const noexcept {
  return m_group->GetRobots();
}
 
 
template <typename GraphType>
Robot*
CompositeState<GraphType>::
GetRobot(const size_t _index) const {
  VerifyIndex(_index);
 
  Robot* const robot = m_group->GetRobot(_index);
 
  if(!robot)
    throw RunTimeException(WHERE) << "Error! Robot pointer was null.";
 
  return robot;
}
 
/*------------------------------ Graph Accessors -----------------------------*/
 
template <typename GraphType>
typename CompositeState<GraphType>::GroupGraphType*
CompositeState<GraphType>::
GetGroupGraph() const noexcept {
  return m_groupMap;
}
 
template <typename GraphType>
void
CompositeState<GraphType>::
SetGroupGraph(GroupGraphType* const _newGraph) {
  // Check that groups are compatible.
  if(m_group != _newGraph->GetGroup())
    throw RunTimeException(WHERE) << "Trying to change graphs on incompatible "
                                  << "groups!";
 
  // Set the group graph and set all vids to invalid.
  m_groupMap = _newGraph;
  m_vids.resize(GetNumRobots(), INVALID_VID);
}
 
template <typename GraphType>
typename CompositeState<GraphType>::VID
CompositeState<GraphType>::
GetVID(const size_t _index) const noexcept {
  VerifyIndex(_index);
  return m_vids[_index];
}
 
template <typename GraphType>
typename CompositeState<GraphType>::VID
CompositeState<GraphType>::
GetVID(Robot* const _robot) const {
  const size_t index = m_groupMap->GetGroup()->GetGroupIndex(_robot);
  return GetVID(index);
}
 
/*------------------------ Individual States -------------------------*/
 
template <typename GraphType>
void
CompositeState<GraphType>::
SetRobotCfg(Robot* const _robot, const VID _vid) {
  if(!m_groupMap)
    throw RunTimeException(WHERE) << "Can't set a VID without a composite graph.";
 
  const size_t index = m_groupMap->GetGroup()->GetGroupIndex(_robot);
  SetRobotCfg(index, _vid);
}
 
 
template <typename GraphType>
void
CompositeState<GraphType>::
SetRobotCfg(const size_t _index, const VID _vid) {
  if(!m_groupMap)
    throw RunTimeException(WHERE) << "Can't set a VID without a composite graph.";
 
  VerifyIndex(_index);
  m_vids[_index] = _vid;
}
 
 
template <typename GraphType>
void
CompositeState<GraphType>::
SetRobotCfg(Robot* const _robot, CfgType&& _cfg) {
  const size_t index = m_group->GetGroupIndex(_robot);
  SetRobotCfg(index, std::move(_cfg));
}
 
 
template <typename GraphType>
void
CompositeState<GraphType>::
SetRobotCfg(const size_t _index, CfgType&& _cfg) {
  VerifyIndex(_index);
 
  m_localCfgs[_index] = std::move(_cfg);
  m_vids[_index] = INVALID_VID;
}
 
 
template <typename GraphType>
void
CompositeState<GraphType>::
ClearLocalCfgs() {
  m_localCfgs.clear();
}
 
 
template <typename GraphType>
typename CompositeState<GraphType>::CfgType&
CompositeState<GraphType>::
GetRobotCfg(Robot* const _robot) {
  const size_t index = m_group->GetGroupIndex(_robot);
  return GetRobotCfg(index);
}
 
 
template <typename GraphType>
typename CompositeState<GraphType>::CfgType&
CompositeState<GraphType>::
GetRobotCfg(const size_t _index) {
  VerifyIndex(_index);
 
  const VID vid = GetVID(_index);
  if(vid != INVALID_VID)
    return m_groupMap->GetIndividualGraph(_index)->GetVertex(vid);
  else {
    InitializeLocalCfgs();
    return m_localCfgs[_index];
  }
}
 
 
template <typename GraphType>
const typename CompositeState<GraphType>::CfgType&
CompositeState<GraphType>::
GetRobotCfg(Robot* const _robot) const {
  const size_t index = m_group->GetGroupIndex(_robot);
  return GetRobotCfg(index);
}
 
 
template <typename GraphType>
const typename CompositeState<GraphType>::CfgType&
CompositeState<GraphType>::
GetRobotCfg(const size_t _index) const {
  VerifyIndex(_index);
 
  // If we have a valid VID for this robot, fetch its state from its
  // individual graph.
  const VID vid = GetVID(_index);
  if(vid != INVALID_VID)
    return m_groupMap->GetIndividualGraph(_index)->GetVertex(vid);
  
  try {
    return m_localCfgs.at(_index);
  }
  catch(const std::out_of_range&) {
    throw RunTimeException(WHERE) << "Requested configuration for robot "
                                  << _index
                                  << ", but no roadmap or local cfg exists.";
  }
}
 
/*----------------------------------------------------------------------------*/
 
template <typename GraphType>
inline
void
CompositeState<GraphType>::
VerifyIndex(const size_t _robotIndex) const noexcept {
  if(_robotIndex >= GetNumRobots())
    throw RunTimeException(WHERE) << "Requested data for robot " << _robotIndex
                                  << ", but the group has only " << GetNumRobots()
                                  << " robots.";
}
 
/*----------------------------------------------------------------------------*/
 
template <typename GraphType>
std::ostream&
operator<<(std::ostream& _os, const CompositeState<GraphType>& _compositeState) {
  // Might not need to be hidden behind GROUP_MAP, but doing for consistency
#ifdef GROUP_MAP
  _os << "0 ";
#endif
 
  // Loop through all robots in the group and print each one's state in order.
  for(size_t i = 0; i < _compositeState.GetNumRobots(); ++i)
    _os << _compositeState.GetRobotCfg(i);
 
  return _os;
}
 
/*----------------------------------------------------------------------------*/
 
template <typename GraphType>
bool
CompositeState<GraphType>::
IsLocalCfg(const size_t _robotIndex) const noexcept {
  // Only true if there is local data (meaning INVALID_VID is present)
  return m_vids[_robotIndex] == INVALID_VID;
}
 
 
template <typename GraphType>
void
CompositeState<GraphType>::
InitializeLocalCfgs() noexcept {
  // We will assume the local cfgs are initialized if the container size is
  // correct.
  const size_t numRobots = GetNumRobots();
  if(m_localCfgs.size() == numRobots)
    return;
 
  m_localCfgs.clear();
  m_localCfgs.resize(numRobots);
 
  for(size_t i = 0; i < numRobots; ++i)
    m_localCfgs[i] = CfgType(this->GetRobot(i));
}
 
/*----------------------------------------------------------------------------*/
 
#endif