incl/rrts.hh

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 /*! \brief RRT* structure and default procedures.
   8  *
   9  * \file
  10  */
  11 #ifndef RRTS_RRTS_H
  12 #define RRTS_RRTS_H
  13
  14 #include <chrono>
  15 #include <functional>
  16 #include <json/json.h>
  17 #include <random>
  18 #include <vector>
  19 #include "bcar.hh"
  20
  21 namespace rrts {
  22 using namespace bcar;
  23
  24 /*! Compute elapsed time class. */
  25 class Ter {
  26 private:
  27         std::chrono::high_resolution_clock::time_point tstart_;
  28 public:
  29         void start();
  30         double scnt() const;
  31 };
  32
  33 /*! Store RRT node. */
  34 class RRTNode : public virtual Pose, public virtual CarMove {
  35 private:
  36         double c_ = 0.0;
  37         double cc_ = 0.0;
  38         RRTNode* p_ = nullptr;
  39         unsigned int cusp_ = 0;
  40 public:
  41         /*! Get cost to parent. */
  42         double c() const;
  43
  44         /*! Set cost to parent. */
  45         void c(double c);
  46
  47         /*! Get cumulative cost from root. */
  48         double cc() const;
  49
  50         /*! Get parent node. */
  51         RRTNode* p() const;
  52
  53         /*! Set parent node. */
  54         void p(RRTNode& p);
  55
  56         /*! Get number of backward-forward direction changes. */
  57         unsigned int cusp() const;
  58
  59         /*! Set number of backward-forward direction changes. */
  60         void cusp(RRTNode const& p);
  61
  62         bool operator==(RRTNode const& n);
  63
  64         int segment_type = 0;
  65 };
  66
  67 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
  68 public:
  69         using PoseRange::PoseRange;
  70 };
  71
  72 /*! RRT* algorithm basic class. */
  73 class RRTS {
  74 protected:
  75         BicycleCar bc_;
  76         RRTGoal goal_;
  77         unsigned int icnt_ = 0;
  78         unsigned int _imax = 1000;
  79         Ter ter_;
  80         std::default_random_engine gen_;
  81         std::vector<RRTNode> nodes_;
  82         std::vector<RRTNode> steered_;
  83         std::vector<RRTNode*> path_;
  84         RRTNode* nn_ = nullptr;
  85         std::vector<RRTNode*> nv_;
  86         double cost_ = 0.0;
  87         double eta_ = 0.5;
  88         double time_ = 0.0;
  89         double last_goal_cc_ = 0.0;
  90         std::vector<RRTNode> last_path_;
  91         void recompute_cc(RRTNode* g);
  92         void recompute_path_cc();
  93         double min_gamma_eta() const;
  94         bool should_continue() const;
  95         void join_steered(RRTNode* f);
  96         RRTNode& nn();
  97         bool connect();
  98         void rewire();
  99         bool goal_drivable_from(RRTNode const& f);
 100         virtual void store(RRTNode n);
 101         virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
 102         virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
 103         virtual void find_nn(RRTNode const& t);
 104         virtual void find_nv(RRTNode const& t);
 105         virtual void compute_path();
 106         virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
 107         virtual bool collide_steered() = 0;
 108         virtual RRTNode sample() = 0;
 109         virtual bool should_finish() const = 0;
 110 public:
 111         RRTS();
 112
 113         /*! Set internal bicycle car. */
 114         BicycleCar &bc();
 115
 116         /*! Set maximum number of iterations before reset. */
 117         void set_imax_reset(unsigned int i);
 118
 119         /*! Set goal. */
 120         void set_goal(double x, double y, double b, double e);
 121
 122         /*! Set start. */
 123         void set_start(double x, double y, double h);
 124
 125         /*! Get path. */
 126         std::vector<Pose> get_path() const;
 127
 128         /*! Get path cost. */
 129         double get_path_cost() const;
 130
 131         /*! Get iterations counter. */
 132         unsigned int icnt() const;
 133
 134         /*! Set iterations counter. */
 135         void icnt(unsigned int i);
 136
 137         /*! Return elapsed time. */
 138         double scnt() const;
 139
 140         double eta() const;
 141         void eta(double e);
 142
 143         /*! Generate JSON output. */
 144         Json::Value json() const;
 145
 146         /*! Load JSON input. */
 147         void json(Json::Value jvi);
 148
 149         /*! Run next RRT* iteration. */
 150         virtual bool next();
 151
 152         /*! Reset the algorithm. */
 153         virtual void reset();
 154 };
 155
 156 } // namespace rrts
 157 #endif /* RRTS_RRTS_H */