DynamicRegionsPRM.h

Go to the documentation of this file.

#ifndef PMPL_DYNAMIC_REGIONS_PRM_H_
#define PMPL_DYNAMIC_REGIONS_PRM_H_
 
#include "MPStrategyMethod.h"
 
#include "Geometry/Boundaries/CSpaceBoundingSphere.h"
#include "MPLibrary/LocalPlanners/LPOutput.h"
#include "MPLibrary/NeighborhoodFinders/Neighbors.h"
#include "Utilities/XMLNode.h"
#include "Workspace/WorkspaceSkeleton.h"
 
 
class DynamicRegionsPRM : public MPStrategyMethod {
 
  public:
 
 
    typedef typename MPBaseObject::WeightType   WeightType;
    typedef typename MPBaseObject::RoadmapType  RoadmapType;
    typedef typename RoadmapType::VID           VID;
    typedef typename RoadmapType::VertexSet     VertexSet;
 
 
    using SkeletonEdgeDescriptor   = WorkspaceSkeleton::ED;
    using SkeletonEdgeIterator     = WorkspaceSkeleton::adj_edge_iterator;
    using SkeletonVertexDescriptor = WorkspaceSkeleton::vertex_descriptor;
    using SkeletonVertexIterator   = WorkspaceSkeleton::vertex_iterator;
 
 
    struct SamplerSetting {
      std::string label;  
      size_t count;       
      size_t attempts;    
    };
 
    struct EdgeOutput {
      VID source;
      VID target;
      std::pair<WeightType, WeightType> weights;
    };
 
    struct LocalComponentDescriptor {
      SkeletonEdgeDescriptor edgeDescriptor; 
      VID representative;                    
      bool bridge;                           
 
      std::ostream& Print(std::ostream& _os) const {
        return _os << "(" << edgeDescriptor.id()
                   << "|"
                   << edgeDescriptor.source() << "," << edgeDescriptor.target()
                   << "|"
                   << representative
                   << "|" << (bridge ? "+" : "-") << ")";
      }
 
      friend std::ostream& operator<<(std::ostream& _os,
          const LocalComponentDescriptor& _d) {
        return _d.Print(_os);
      }
    };
 
    struct ExpansionRegion {
 
 
      const SkeletonEdgeIterator edgeIterator; 
      VID representative{INVALID_VID};       
      size_t edgeIndex{0};   
      double attempts{1};    
      double successes{0};   
 
 
      ExpansionRegion(const SkeletonEdgeIterator& _eit, const VID _representative)
          : edgeIterator(_eit), representative(_representative) {}
 
      void TrackSuccess(const size_t _success, const size_t _attempts) {
        successes *= .9;
        attempts  *= .9;
        successes += _success;
        attempts  += _attempts;
      }
 
      double GetWeight() const noexcept {
        return successes / attempts;
      }
 
      const Vector3d& GetCenter() const noexcept {
        return (*edgeIterator).property()[edgeIndex];
      }
 
      bool LastPoint() const noexcept {
        return edgeIndex == (*edgeIterator).property().size() - 1;
      }
 
      void Advance() noexcept {
        ++edgeIndex;
      }
 
      LocalComponentDescriptor GetLocalComponentDescriptor() const noexcept {
        return {(*edgeIterator).descriptor(), representative, false};
      }
 
    };
 
    struct EdgeCompare {
      bool
      operator()(const SkeletonEdgeDescriptor& _d1,
          const SkeletonEdgeDescriptor& _d2) const noexcept {
        // Test ID first since we should only ever have two edges on the same ID (one
        // for each direction). If ID are equal, sort by source, then target.
        return _d1.id() < _d2.id()
            or (!(_d1.id() > _d2.id())
                and (_d1.source() < _d2.source()
                     or (!(_d1.source() > _d2.source())
                         and _d1.target() < _d2.target()
                        )
                    )
               );
      }
    };
 
    struct EdgeIDCompare {
      bool
      operator()(const SkeletonEdgeDescriptor& _d1,
          const SkeletonEdgeDescriptor& _d2) const noexcept {
        return _d1.id() < _d2.id();
      }
    };
 
    typedef std::map<Robot*, std::map<SkeletonEdgeDescriptor, std::map<VID, VertexSet>, EdgeCompare>>
        LocalConnectivityMap;
 
    typedef std::map<Robot*, std::map<SkeletonEdgeDescriptor, std::map<VID, VertexSet>, EdgeIDCompare>>
        BridgeMap;
 
    typedef std::map<Robot*, std::map<SkeletonEdgeDescriptor, std::map<VID, ExpansionRegion>, EdgeCompare>>
        RegionMap;
 
    typedef std::map<Robot*, std::set<size_t>> LowClearanceMap;
 
    typedef std::map<Robot*, std::set<SkeletonEdgeDescriptor, EdgeCompare>> UnconnectedEdgeMap;
 
 
    DynamicRegionsPRM();
 
    DynamicRegionsPRM(XMLNode& _node);
 
    virtual ~DynamicRegionsPRM() = default;
 
 
    virtual void Print(std::ostream& _os) const override;
 
 
    virtual void Initialize() override;
 
    virtual void Iterate() override;
 
 
  protected:
 
 
    void AddQuery();
 
    bool CheckReachedRegion(const VID _query, const VertexSet& _component);
 
    void GrowRRT(const VID _q);
 
 
    VertexSet ExpandComponent(ExpansionRegion* const _r);
 
    void ConnectEdgeSegment(const SkeletonEdgeDescriptor _ed);
 
    // Initialize the unconnected edge map
    void InitializeLUnconnectedEdgeMap();
 
 
    void MakeLocalComponent(const LocalComponentDescriptor& _d,
        const VertexSet& _vids);
 
    VertexSet& GetLocalComponent(const LocalComponentDescriptor& _d) noexcept;
 
    ExpansionRegion* GetExpansionRegion(const LocalComponentDescriptor& _d)
        noexcept;
 
    VertexSet& GetBridge(const LocalComponentDescriptor& _d) noexcept;
 
    LocalComponentDescriptor FindLocalComponent(const VID _vid) const noexcept;
 
    LocalComponentDescriptor PromoteLocalComponent(LocalComponentDescriptor _d);
 
    LocalComponentDescriptor MergeLocalComponents(
        const LocalComponentDescriptor& _d1,
        const LocalComponentDescriptor& _d2);
 
    void UpdateEdgeConnectivity(const SkeletonEdgeDescriptor& _ed);
 
    bool IsEdgeConnected(const SkeletonEdgeDescriptor& _ed);
 
 
    ExpansionRegion* SelectExpansionRegion();
 
    std::pair<std::vector<double>, std::vector<ExpansionRegion*>>
        ComputeProbabilities();
 
    bool AdvanceRegion(ExpansionRegion* const _r,
        const VertexSet& _newVIDs);
 
    bool AreSamplesCovered(const ExpansionRegion* const _region,
        const VertexSet& _samples);
 
 
    std::vector<Cfg> Sample(const Boundary* _b = nullptr);
 
    std::vector<Neighbor> FindNearestNeighbors(const Cfg& _cfg,
        const VertexSet* const _candidates);
 
    bool AttemptConnection(const Cfg& _c1, const Cfg& _c2,
        LPOutput& _lpOuptut);
 
    std::vector<EdgeOutput> ConnectToComponent(const Cfg& _cfg,
        const LocalComponentDescriptor& _d);
 
    VID Extend(const VID _nearVID, const Cfg& _target,
        LPOutput& _lp);
 
 
    CSpaceBoundingSphere MakeBoundary(const Vector3d& _v);
 
    double GetRegionRadius(const Vector3d& _v);
 
    double GetClearance(const Vector3d& _v);
 
    void BuildSkeleton();
 
    // Initialize the low clearance map
    void InitializeLowClearanceMap();
 
 
    std::vector<SamplerSetting> m_samplers;     
    std::string m_nfLabel;            
    std::string m_lpLabel;            
    std::string m_exLabel;            
    std::string m_decompositionLabel; 
 
    std::string m_skeletonType{"reeb"}; 
    std::string m_skeletonFilename;  
    std::string m_skeletonIO;        
 
    double m_explore{.5};
 
    double m_minRegionRadius{-1};
 
    bool m_aggressiveBridging{true};
 
 
    static bool m_initialized; 
 
    static WorkspaceSkeleton m_skeleton;     
 
    static LocalConnectivityMap m_localComponents; 
    static BridgeMap m_bridges;                    
 
    static RegionMap m_regions; 
 
    static LowClearanceMap m_lowClearanceMap; 
 
    static UnconnectedEdgeMap m_unconnectedEdges; 
 
 
};
 
#endif