rrts/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_cnt = 0;
  40         int _segment_type = 0;  // 0 ~ straight, 1 ~ left, -1 right
  41         bool _p_is_cusp = false;
  42 public:
  43         /*! Get cost to parent. */
  44         double c() const;
  45
  46         /*! Set cost to parent. */
  47         void c(double c);
  48
  49         /*! Get cumulative cost from root. */
  50         double cc() const;
  51
  52         /*! Get parent node. */
  53         RRTNode* p() const;
  54
  55         /*! Set parent node. */
  56         void p(RRTNode& p);
  57
  58         /*! Get number of backward-forward direction changes.
  59          *
  60          * The parent node may be cusp to this node, i.e. parent may have sp ==
  61          * 0 or sgn(sp) differs from sgn(this->sp). In such a situation, the
  62          * number of this->cusp_cnt is incremented by one.
  63          */
  64         unsigned int cusp_cnt() const;
  65
  66         /*! Set number of backward-forward direction changes. */
  67         void cusp_cnt(RRTNode const& p);
  68
  69         /*! \brief Get Reeds & Shepp segment type.
  70          *
  71          * Segment type is:
  72          * - 0 for straight,
  73          * - 1 for turning left,
  74          * - and -1 for turning right.
  75          */
  76         int st(void);
  77
  78         /*! Set Reeds & Shepp segment type. */
  79         void st(int st);
  80
  81         /*! Return true if the parent is cusp node and false otherwise. */
  82         bool p_is_cusp(void) const;
  83
  84         /*! Set if the parent node is cusp (direction changed from parent). */
  85         void p_is_cusp(bool isit);
  86
  87         bool operator==(RRTNode const& n);
  88 };
  89
  90 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
  91 public:
  92         using PoseRange::PoseRange;
  93 };
  94
  95 /*! RRT* algorithm basic class. */
  96 class RRTS {
  97 protected:
  98         BicycleCar _bc;
  99         RRTGoal _goal;
 100         unsigned int _icnt = 0;
 101         unsigned int _icnt_max = 1000;
 102         Ter _ter;
 103         std::default_random_engine _gen;
 104         std::vector<RRTNode> _nodes;
 105         std::vector<RRTNode> _steered;
 106         std::vector<RRTNode*> _path;
 107         RRTNode* _nn = nullptr;
 108         std::vector<RRTNode*> _nv;
 109         double _cost = 0.0;
 110         double _eta = 0.5;
 111         double _time = 0.0;
 112         std::vector<std::vector<RRTNode>> _logged_paths;
 113 protected:
 114         double min_gamma_eta(void) const;
 115         bool should_continue(void) const;
 116         void recompute_cc_for_predecessors_and(RRTNode* g);
 117         void recompute_path_cc();
 118         void join_steered(RRTNode* f);
 119         bool connect();
 120         void rewire();
 121         bool goal_drivable_from(RRTNode const& f);
 122 protected:
 123         virtual void store(RRTNode n);
 124         virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
 125         virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
 126         virtual void find_nn(RRTNode const& t);
 127         virtual void find_nv(RRTNode const& t);
 128         virtual void compute_path();
 129 protected:
 130         /*! \brief Return nodes from f to t inclusive.
 131          *
 132          * The "inclusive" means that f is at the same pose (x, y, h) as
 133          * this->_steered.front() and t is at the same pose (x, y, h) as
 134          * this->_steered.back().
 135          */
 136         virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
 137         virtual bool collide_steered() = 0;
 138         virtual RRTNode sample() = 0;
 139         virtual bool should_finish() const = 0;
 140 public:
 141         RRTS();
 142
 143         /*! Set pose of the bicycle car used in the planner. */
 144         void set_bc_pose_to(Pose const& p);
 145
 146         /*! Set bicycle car dimensions. */
 147         void set_bc_to_become(std::string what);
 148
 149         /*! Get goal. */
 150         RRTGoal const& goal(void) const;
 151
 152         /*! Set goal. */
 153         void goal(double x, double y, double b, double e);
 154
 155         /*! Get number of iterations. */
 156         unsigned int icnt(void) const;
 157
 158         /*! Set number of iterations. */
 159         void icnt(unsigned int i);
 160
 161         /*! Get maximum number of iterations before reset. */
 162         unsigned int icnt_max(void) const;
 163
 164         /*! Set maximum number of iterations before reset. */
 165         void icnt_max(unsigned int i);
 166
 167         /*! Start elapsed time counter. */
 168         void tstart(void);
 169
 170         /*! Return elapsed time. */
 171         double scnt() const;
 172
 173         /*! Set init pose. */
 174         void set_init_pose_to(Pose const& p);
 175
 176         /*! Get path. */
 177         std::vector<Pose> path() const;
 178
 179         /*! Get path cost. */
 180         double path_cost() const;
 181
 182         /*! Get cost of the last path. */
 183         double last_path_cost(void) const;
 184
 185         /*! Get eta, the RRT* constant used in near vertices and steering. */
 186         double eta() const;
 187
 188         /*! Set eta. */
 189         void eta(double e);
 190 public:
 191         /*! Generate JSON output. */
 192         virtual Json::Value json(void) const;
 193
 194         /*! Load JSON input. */
 195         virtual void json(Json::Value jvi);
 196
 197         /*! Run next RRT* iteration. Return True if should continue. */
 198         virtual bool next();
 199
 200         /*! Reset the algorithm. */
 201         virtual void reset();
 202 };
 203
 204 } // namespace rrts
 205 #endif /* RRTS_RRTS_H */