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 last_goal_cc_ = 0.0;
  74         double min_gamma_eta() const;
  75         bool should_continue() const;
  76         void join_steered(RRTNode* f);
  77         RRTNode& nn();
  78         bool connect();
  79         void rewire();
  80         bool goal_drivable_from(RRTNode const& f);
  81         virtual void store(RRTNode n);
  82         virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
  83         virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
  84         virtual void find_nn(RRTNode const& t);
  85         virtual void find_nv(RRTNode const& t);
  86         virtual void compute_path();
  87         virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
  88         virtual bool collide_steered() = 0;
  89         virtual RRTNode sample() = 0;
  90         virtual bool should_finish() const = 0;
  91 public:
  92         RRTS();
  93
  94         /*! Get iterations counter. */
  95         unsigned int icnt() const;
  96
  97         /*! Set iterations counter. */
  98         void icnt(unsigned int i);
  99
 100         /*! Return elapsed time. */
 101         double scnt() const;
 102
 103         /*! Generate JSON output. */
 104         Json::Value json() const;
 105
 106         /*! Load JSON input. */
 107         void json(Json::Value jvi);
 108
 109         /*! Run next RRT* iteration. */
 110         virtual bool next();
 111
 112         /*! Reset the algorithm. */
 113         virtual void reset();
 114 };
 115
 116 } // namespace rrts
 117 #endif /* RRTS_RRTS_H */