incl/rrts.hh

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