AdaptiveRRT.h

Go to the documentation of this file.

#ifndef ADAPTIVE_RRT_H_
#define ADAPTIVE_RRT_H_
 
#include "BasicRRTStrategy.h"
 
// assembly code to measure cpu cycles
#include <cstdint>
#if defined(__x86_64__) || defined(_M_X64) // For x86_64 architecture
static inline uint64_t GetCycles() {
    uint64_t n;
    __asm__ __volatile__("rdtsc" : "=A"(n));
    return n;
}
#elif defined(__aarch64__) // For ARM64 architecture, like Apple Silicon
#include <time.h>
static inline uint64_t GetCycles() {
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return static_cast<uint64_t>(ts.tv_sec) * 1000000000ULL + ts.tv_nsec;
}
#else
#error "Unsupported architecture"
#endif
 
 
class AdaptiveRRT : public BasicRRTStrategy {
  public:
 
    // GrowthSet: map<Growth Method, pair<Weight Tuple, Cost>>
    typedef std::map<std::string, std::pair<std::pair<double, long>, double>> GrowthSet;
    // GrowthSets: map<Visibility Threshold, GrowthSet>
    typedef std::map<double, GrowthSet> GrowthSets;
 
    typedef typename MPBaseObject::WeightType   WeightType;
    typedef typename MPBaseObject::RoadmapType  RoadmapType;
    typedef typename RoadmapType::VID           VID;
 
 
    //cost calculation method for AdaptiveRRT
    enum CostMethod {FIXED, REWARD, CYCLES};
 
 
    AdaptiveRRT(double _wallPenalty = 0.5, double _gamma = 0.5,
        const GrowthSets& _growthSets = GrowthSets(), CostMethod _c = FIXED);
 
    AdaptiveRRT(XMLNode& _node);
 
    //AdaptiveRRT();
 
    virtual ~AdaptiveRRT() = default;
 
 
    virtual void Print(std::ostream& _os) const;
 
 
 
  protected:
 
    virtual void Initialize() override;
 
 
    virtual VID ExpandTree(Cfg& _dir);
 
    std::string SelectGrowthMethod(GrowthSet& _gs);
 
    void UpdateCost(double _cost, std::string _s, GrowthSet& _gs);
 
    void RewardGrowthMethod(double _r, std::string _s, GrowthSet& _gs);
 
 
    VID UpdateTree(VID _nearest, Cfg& _new, Cfg& _dir, double _delta);
 
    VID AddNode(Cfg& _newCfg, VID _nearVID, bool _againstWall,
        double _ratio);
 
 
 
  private:
 
    void AvgVisibility(Cfg& _cfg, double _val);
 
    double CostInsensitiveProb(std::string _s, GrowthSet& _gs);
 
 
    double GetVisibility(Cfg& _cfg);
 
    double GetCost(std::string _s, GrowthSet& _gs);
 
 
    double m_wallPenalty;      
 
    double m_gamma;            
 
    GrowthSets m_growthSets;   
 
    CostMethod m_costMethod;   
 
};
 
#endif