incl/rrts.hh

   1 /*! \file */
   2 #ifndef RRTS_RRTS_H
   3 #define RRTS_RRTS_H
   4
   5 #include <chrono>
   6 #include <functional>
   7 #include <json/json.h>
   8 #include <random>
   9 #include <vector>
  10 #include "bcar.hh"
  11
  12 namespace rrts {
  13 using namespace bcar;
  14
  15 /*! Compute elapsed time class. */
  16 class Ter {
  17 private:
  18         std::chrono::high_resolution_clock::time_point tstart_;
  19 public:
  20         void start();
  21         double scnt() const;
  22 };
  23
  24 /*! Store RRT node. */
  25 class RRTNode : public virtual Pose, public virtual CarMove {
  26 private:
  27         double c_ = 0.0;
  28         double cc_ = 0.0;
  29         RRTNode* p_ = nullptr;
  30 public:
  31         /*! Get cost to parent. */
  32         double c() const;
  33
  34         /*! Set cost to parent. */
  35         void c(double c);
  36
  37         /*! Get cumulative cost from root. */
  38         double cc() const;
  39
  40         /*! Get parent node. */
  41         RRTNode* p() const;
  42
  43         /*! Set parent node. */
  44         void p(RRTNode& p);
  45
  46         bool operator==(RRTNode const& n);
  47 };
  48
  49 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
  50 };
  51
  52 /*! RRT* algorithm basic class. */
  53 class RRTS {
  54 protected:
  55         BicycleCar bc_;
  56         RRTGoal goal_;
  57         unsigned int icnt_ = 0;
  58         Ter ter_;
  59         std::default_random_engine gen_;
  60         std::vector<RRTNode> nodes_;
  61         std::vector<RRTNode> steered_;
  62         std::vector<RRTNode*> path_;
  63         RRTNode* nn_ = nullptr;
  64         std::vector<RRTNode*> nv_;
  65         double cost_ = 0.0;
  66         double eta_ = 0.5;
  67         double time_ = 0.0;
  68         double min_gamma_eta() const;
  69         bool should_continue() const;
  70         void join_steered(RRTNode* f);
  71         RRTNode& nn();
  72         bool connect();
  73         void rewire();
  74         bool goal_drivable_from(RRTNode const& f);
  75         virtual void store(RRTNode n);
  76         virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
  77         virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
  78         virtual void find_nn(RRTNode const& t);
  79         virtual void find_nv(RRTNode const& t);
  80         virtual void compute_path();
  81         virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
  82         virtual bool collide_steered() = 0;
  83         virtual RRTNode sample() = 0;
  84         virtual bool should_finish() const = 0;
  85 public:
  86         RRTS();
  87
  88         /*! Get iterations counter. */
  89         unsigned int icnt() const;
  90
  91         /*! Set iterations counter. */
  92         void icnt(unsigned int i);
  93
  94         /*! Return elapsed time. */
  95         double scnt() const;
  96
  97         /*! Generate JSON output. */
  98         Json::Value json() const;
  99
 100         /*! Load JSON input. */
 101         void json(Json::Value jvi);
 102
 103         /*! Run next RRT* iteration. */
 104         virtual bool next();
 105
 106         /*! Reset the algorithm. */
 107         virtual void reset();
 108 };
 109
 110 } // namespace rrts
 111 #endif /* RRTS_RRTS_H */