ConnectorMethod.h

Go to the documentation of this file.

#ifndef PMPL_CONNECTION_METHOD_H_
#define PMPL_CONNECTION_METHOD_H_
 
#include "ConfigurationSpace/Weight.h"
 
#include "MPLibrary/LocalPlanners/LocalPlannerMethod.h"
#include "MPLibrary/NeighborhoodFinders/NeighborhoodFinderMethod.h"
#include "MPLibrary/MPBaseObject.h"
#include "Utilities/Hash.h"
#include "Utilities/MPUtils.h"
#include "Utilities/MetricUtils.h"
 
#include <iterator>
#include <set>
 
 
class ConnectorMethod : public MPBaseObject
{
  public:
 
 
    typedef typename MPBaseObject::GroupCfgType      GroupCfgType;
    typedef typename MPBaseObject::RoadmapType       RoadmapType;
    typedef typename MPBaseObject::GroupRoadmapType  GroupRoadmapType;
    typedef typename RoadmapType::VID            VID;
    typedef typename RoadmapType::VertexSet      VertexSet;
 
 
    typedef std::pair<VID, VID>                    ConnectionAttempt;
    typedef std::unordered_set<ConnectionAttempt>  ConnectionAttempts;
    typedef std::unordered_map<void*, ConnectionAttempts> ConnectionAttemptsCache;
 
    template <typename AbstractRoadmapType>
    using OutputIterator = std::back_insert_iterator<
                             std::vector<
                               typename AbstractRoadmapType::VP
                             >
                           >;
 
 
    ConnectorMethod();
 
    ConnectorMethod(XMLNode& _node);
 
    virtual ~ConnectorMethod() = default;
 
 
    virtual void Print(std::ostream& _os) const override;
 
    virtual void Initialize() override;
 
 
    template <typename AbstractRoadmapType>
    void Connect(AbstractRoadmapType* const _r,
        const VertexSet* const _targetSet = nullptr,
        OutputIterator<AbstractRoadmapType>* const _collision = nullptr);
 
    template <typename AbstractRoadmapType>
    void Connect(AbstractRoadmapType* const _r, const VID _source,
        const VertexSet* const _targetSet = nullptr,
        OutputIterator<AbstractRoadmapType>* const _collision = nullptr);
 
    template <typename AbstractRoadmapType, typename InputIterator>
    void Connect(AbstractRoadmapType* const _r,
        InputIterator _sourceBegin, InputIterator _sourceEnd,
        const VertexSet* const _targetSet = nullptr,
        OutputIterator<AbstractRoadmapType>* const _collision = nullptr);
 
    bool IsRewiring() const noexcept;
 
 
  protected:
 
 
    virtual void ConnectImpl(RoadmapType* const _r, const VID _source,
        const VertexSet* const _targetSet = nullptr,
        OutputIterator<RoadmapType>* const _collision = nullptr);
 
    virtual void ConnectImpl(GroupRoadmapType* const _r, const VID _source,
        const VertexSet* const _targetSet = nullptr,
        OutputIterator<GroupRoadmapType>* const _collision = nullptr);
 
 
    template <typename AbstractRoadmapType>
    void ConnectNeighbors(
        AbstractRoadmapType* const _r, const VID _source,
        const std::vector<Neighbor>& _neighbors,
        OutputIterator<AbstractRoadmapType>* const _collision = nullptr,
        const bool _earlyQuit = false);
 
    bool ConnectNodes(RoadmapType* const _r, const VID _source,
        const VID _target,
        OutputIterator<RoadmapType>* const _collision = nullptr);
 
    bool ConnectNodes(GroupRoadmapType* const _r, const VID _source,
        const VID _target,
        OutputIterator<GroupRoadmapType>* const _collision = nullptr);
 
    template <typename AbstractRoadmapType>
    bool DoNotCheck(AbstractRoadmapType* const _r, const VID _source,
        const VID _target) const;
 
 
    void CacheFailedConnection(void* const _map, const VID _source,
        const VID _target) noexcept;
 
    bool IsCached(void* const _map, const VID _source, const VID _target) const
        noexcept;
 
    void ClearConnectionAttempts();
 
 
    ConnectionAttemptsCache m_attemptsCache; 
    std::string m_lpLabel;                   
 
    bool m_skipIfSameCC{true};  
    size_t m_maxFailures{0};    
    size_t m_failures{0};       
    bool m_oneWay{false};       
    bool m_rewiring{false};     
 
        bool m_selfEdges{false};        
 
    std::vector<Neighbor> m_neighborBuffer;
 
 
};
 
 
template <typename AbstractRoadmapType>
void
ConnectorMethod::
Connect(AbstractRoadmapType* const _r, const VertexSet* const _targetSet,
    OutputIterator<AbstractRoadmapType>* const _collision) {
  Connect(_r, _r->begin(), _r->end(), _targetSet, _collision);
}
 
 
template <typename AbstractRoadmapType>
void
ConnectorMethod::
Connect(AbstractRoadmapType* const _r, const VID _source,
    const VertexSet* const _targetSet,
    OutputIterator<AbstractRoadmapType>* const _collision)
{
  const std::array<VID, 1> source{_source};
  Connect(_r, source.begin(), source.end(), _targetSet, _collision);
}
 
 
template <typename AbstractRoadmapType, typename InputIterator>
void
ConnectorMethod::
Connect(AbstractRoadmapType* const _r,
    InputIterator _sourceBegin, InputIterator _sourceEnd,
    const VertexSet* const _targetSet,
    OutputIterator<AbstractRoadmapType>* const _collision)
{
  const std::string id = this->GetNameAndLabel() + "::Connect";
  MethodTimer mt(this->GetStatClass(), id);
  if(this->m_debug)
    std::cout << id << std::endl;
 
  m_failures = 0;
  for(auto iter = _sourceBegin; iter != _sourceEnd; ++iter) {
    const VID sourceVID = _r->GetVID(iter);
    if(this->m_debug)
      std::cout << "\tAttempting connections from node "
                << sourceVID << " at " << _r->GetVertex(sourceVID).PrettyPrint()
                << std::endl;
    ConnectImpl(_r, sourceVID, _targetSet, _collision);
  }
}
 
 
template <typename AbstractRoadmapType>
void
ConnectorMethod::
ConnectNeighbors(AbstractRoadmapType* const _r, const VID _source,
    const std::vector<Neighbor>& _neighbors,
    OutputIterator<AbstractRoadmapType>* const _collision,
    const bool _earlyQuit) {
  // Try to connect _source to each neighbor.
  for(const auto& neighbor : _neighbors) {
    // Check if we have exceeded the failure limit.
    if(m_maxFailures and m_failures >= m_maxFailures) {
      if(this->m_debug)
        std::cout << "\t\tFailures (" << m_failures << ") exceed threshold "
                  << "of " << m_maxFailures << ", stopping."
                  << std::endl;
      return;
    }
 
    // Attempt the next connection.
    const VID target = neighbor.target;
    if(this->m_debug)
      std::cout << "\t\tAttempting connection from " << _source << " to "
                << target << " at distance " << neighbor.distance << "."
                << std::endl;
 
    // Check if this attempt should be skipped.
    if(DoNotCheck(_r, _source, target))
      continue;
 
    // Attempt connection with the local planner.
    const bool connected = ConnectNodes(_r, _source, target, _collision);
    m_failures += !connected;
 
    if(this->m_debug)
      std::cout << "\t\t\tConnection " << (connected ? "succeeded" : "failed")
                << "." << std::endl;
 
    // Quit early on success if requested.
    if(_earlyQuit && connected)
      return;
  }
 
    //Connect vertices to themselves
    //if(m_selfEdges) {
        //this->ConnectNodes(_r, _source, _source, _collision);
    //}
}
 
 
template <typename AbstractRoadmapType>
bool
ConnectorMethod::
DoNotCheck(AbstractRoadmapType* const _r, const VID _source, const VID _target)
    const {
  // Check that both VIDs are valid.
  if(_source == INVALID_VID or _target == INVALID_VID) {
    if(this->m_debug)
      std::cout << "\t\t\tSkipping connection with invalid node "
                << "(" << _source << ", " << _target << ")."
                << std::endl;
    return true;
  }
 
  // Check for self-connection.
  if(_source == _target and !m_selfEdges) {
    if(this->m_debug)
      std::cout << "\t\t\tSkipping self-connection "
                << "(" << _source << ", " << _target << ")."
                << std::endl;
    return true;
  }
 
  // Check for cached connection.
  if(this->IsCached(_r, _source, _target)) {
    if(this->m_debug)
      std::cout << "\t\t\tSkipping cached failed connection "
                << "(" << _source << ", " << _target << ")."
                << std::endl;
    return true;
  }
 
  // Check if the edge already exists.
  if(_r->IsEdge(_source, _target)) {
    if(this->m_debug)
      std::cout << "\t\t\tSkipping existing connection "
                << "(" << _source << ", " << _target << ")."
                << std::endl;
    return true;
  }
 
  // Check if the nodes are in the same connected component.
  if(m_skipIfSameCC) {
    auto ccTracker = _r->GetCCTracker();
    if(!ccTracker)
      throw RunTimeException(WHERE) << "A CCTracker is required for checking "
                                    << "same CCs.";
    const bool sameCC = ccTracker->InSameCC(_source, _target);
    if(sameCC) {
      if(this->m_debug)
        std::cout << "\t\t\tSkipping connection "
                  << "(" << _source << ", " << _target << ")"
                  << " within the same connected component."
                  << std::endl;
      return true;
    }
  }
 
  return false;
}
 
#endif