BasicRRTStrategy.h

Go to the documentation of this file.

#ifndef PMPL_BASIC_RRT_STRATEGY_H_
#define PMPL_BASIC_RRT_STRATEGY_H_
 
#include "MPLibrary/LocalPlanners/LPOutput.h"
#include "MPLibrary/NeighborhoodFinders/Neighbors.h"
#include "MPStrategyMethod.h"
#include "Utilities/XMLNode.h"
 
// #include <iomanip>
// #include <iterator>
// #include <string>
// #include <unordered_set>
// #include <vector>
 
 
class BasicRRTStrategy : public MPStrategyMethod {
 
  public:
 
 
    typedef size_t VID;
    typedef typename std::unordered_set<VID> VertexSet;
 
 
    BasicRRTStrategy();
 
    BasicRRTStrategy(XMLNode& _node);
 
    virtual ~BasicRRTStrategy();
 
 
    virtual void Print(std::ostream& _os) const;
 
 
  protected:
 
 
    virtual void Initialize() override;
    virtual void Iterate() override;
 
 
    virtual Cfg SelectTarget();
 
    Cfg SelectDispersedTarget(const VID _v);
 
 
    virtual VID FindNearestNeighbor(const Cfg& _cfg,
        const VertexSet* const _candidates = nullptr);
 
    virtual Neighbor SelectNeighbor(const Cfg& _cfg,
        const std::vector<Neighbor>& _neighbors);
 
 
    virtual VID Extend(const VID _nearVID, const Cfg& _target,
        LPOutput& _lp, const bool _requireNew = true);
 
    VID Extend(const VID _nearVID, const Cfg& _target,
        const bool _requireNew = true);
 
    virtual std::pair<VID, bool> AddNode(const Cfg& _newCfg);
 
    virtual void AddEdge(const VID _source, const VID _target,
        const LPOutput& _lpOutput);
 
    void ConnectNeighbors(const VID _newVID);
 
    void TryGoalExtension(const VID _newVID);
 
    void TryGoalExtension(const VID _newVID, const Boundary* const _boundary);
 
 
    VID ExpandTree(const Cfg& _target);
 
    virtual VID ExpandTree(const VID _nearestVID, const Cfg& _target);
 
    void ConnectTrees(const VID _recentlyGrown);
 
 
    std::string m_samplerLabel;  
    std::string m_nfLabel;       
    std::string m_ncLabel;       
    std::string m_exLabel;       
    std::string m_goalDmLabel;   
 
    std::string m_fallbackNfLabel; 
 
 
    bool m_growGoals{false};      
    double m_growthFocus{0};      
    double m_goalThreshold{0};    
    size_t m_numDirections{1};    
    size_t m_disperseTrials{3};   
 
 
    std::vector<VertexSet> m_trees;  
 
 
};
 
#endif