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