rrts/incl/rrtext.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* extensions.
   8  *
   9  * The extensions are used to implement or change the default behavior of the
  10  * RRT* algorithm.
  11  *
  12  * \file
  13  * \defgroup ext-col Collision detection extensions
  14  * \defgroup ext-store Node storage and searching tree extensions
  15  * \defgroup ext-cost Cost extensions
  16  * \defgroup ext-opt Path optimization extensions
  17  * \defgroup ext-sampl Random sampling extensions
  18  * \defgroup ext-steer Steering procedure extensions
  19  * \defgroup ext-aux Auxilliary extensions
  20  */
  21 #ifndef RRTS_RRTEXT_H
  22 #define RRTS_RRTEXT_H
  23
  24 #include "rrts.hh"
  25
  26 // ext2
  27 #include "cute_c2.h"
  28
  29 // ext4
  30 #define GRID 1 // in meters
  31 #define GRID_WIDTH 40 // in meters
  32 #define GRID_HEIGHT 40 // in meters
  33 #define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
  34 #define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
  35
  36 // ext9
  37 #define GRID_MAX_HI 60
  38
  39 namespace rrts {
  40
  41 /*! \brief Collision check based on enlarged occupancy grid.
  42  *
  43  * \ingroup ext-col
  44  */
  45 class RRTExt21 : public virtual RRTS {
  46 private:
  47         unsigned int _grid_width = 0;
  48         unsigned int _grid_height = 0;
  49         float _grid_res = 0.0;
  50         int8_t const *_grid_data = nullptr;
  51         double _origin_x = 0.0;
  52         double _origin_y = 0.0;
  53         bool collide(RRTNode const &n);
  54         bool collide_steered();
  55 public:
  56         void set_grid_to_check(unsigned int w, unsigned int h, float r,
  57                         int8_t const *d, double x, double y);
  58 };
  59
  60 /*! \brief Use Dubins paths-based steering procedure.
  61  *
  62  * \ingroup ext-steer
  63  * \see https://github.com/AndrewWalker/Dubins-Curves
  64  */
  65 class RRTExt19 : public virtual RRTS {
  66 private:
  67         void steer(RRTNode const &f, RRTNode const &t);
  68 };
  69
  70 /*! \brief Finish when more than 1000 iterations.
  71  *
  72  * \ingroup ext-aux
  73  */
  74 class RRTExt18 : public virtual RRTS {
  75 private:
  76         bool should_finish() const;
  77 };
  78
  79 /*! \brief Finish when goal found or more than 1000 iterations.
  80  *
  81  * \ingroup ext-aux
  82  */
  83 class RRTExt17 : public virtual RRTS {
  84 private:
  85         bool should_finish() const;
  86 };
  87
  88 /*! \brief Use Reeds & Shepp steering procedure.
  89  *
  90  * \ingroup ext-steer
  91  */
  92 class RRTExt16 : public virtual RRTS {
  93 private:
  94         void steer(RRTNode const& f, RRTNode const& t);
  95 };
  96
  97 /*! \brief Random sampling in the circuit between root and goal.
  98  *
  99  * \ingroup ext-sampl
 100  * \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
 101  */
 102 class RRTExt14 : public virtual RRTS {
 103 private:
 104         Point circle_c_;
 105         double circle_r_ = 0.0;
 106         std::uniform_real_distribution<double> udr_;
 107         std::uniform_real_distribution<double> udt_;
 108         std::uniform_real_distribution<double> udh_;
 109         RRTNode sample();
 110 public:
 111         RRTExt14();
 112         void reset();
 113 };
 114
 115 /*! \brief Use Dijkstra algorithm to find path between interesting nodes.
 116  *
 117  * The search for interesting nodes starts at root, the last drivable nodes is
 118  * added to interesting nodes until goal is reached.
 119  *
 120  * \ingroup ext-opt
 121  */
 122 class RRTExt13 : public virtual RRTS {
 123 private:
 124         class DijkstraNode {
 125         public:
 126                 RRTNode* p_rrtnode = nullptr;
 127                 unsigned int i = 0;
 128                 bool v = false;
 129                 double d = 0.0;
 130                 bool vi()
 131                 {
 132                         if (this->v) {
 133                                 return true;
 134                         }
 135                         this->v = true;
 136                         return false;
 137                 }
 138                 DijkstraNode(RRTNode* n) : p_rrtnode(n) {}
 139         };
 140         class DijkstraNodeComparator {
 141         public:
 142                 int operator() (DijkstraNode const& n1, DijkstraNode const& n2)
 143                 {
 144                         return n1.p_rrtnode->cc() > n2.p_rrtnode->cc();
 145                 }
 146         };
 147         class DijkstraNodeBackwardComparator {
 148         public:
 149                 int operator() (DijkstraNode const& n1, DijkstraNode const& n2)
 150                 {
 151                         return n1.d > n2.d;
 152                 }
 153         };
 154         std::vector<RRTNode*> opath_;
 155         double ogoal_cc_ = 0.0;
 156         double otime_ = 0.0;
 157         std::vector<DijkstraNode> dn_;
 158         void interesting_forward();
 159         void interesting_backward();
 160         void dijkstra_forward();
 161         void dijkstra_backward();
 162         void compute_path(void) override;
 163 public:
 164         RRTExt13();
 165         Json::Value json(void) const override;
 166         void reset(void) override;
 167 };
 168
 169 /*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
 170  *
 171  * Use Reeds & Shepp path length for building tree data structure and Euclidean
 172  * distance + (abs) heading difference + 0.1 * backward-forward direction
 173  * changes for searching it.
 174  *
 175  * \ingroup ext-cost
 176  * \see https://doi.org/10.1109/TITS.2015.2477355
 177  */
 178 class RRTExt10 : public virtual RRTS {
 179 protected:
 180         double cost_build(RRTNode const& f, RRTNode const& t) const;
 181         double cost_search(RRTNode const& f, RRTNode const& t) const;
 182 };
 183
 184 /*! \brief Use 3D k-d tree data structure to store RRT nodes.
 185  *
 186  * This approach speeds up the search process for the nearest neighbor and the
 187  * near vertices procedures. This extension implements 3D K-d tree.
 188  *
 189  * \ingroup ext-store
 190  * \see https://en.wikipedia.org/wiki/K-d_tree
 191  */
 192 class RRTExt8 : public virtual RRTS {
 193 private:
 194         class KdNode {
 195         public:
 196                 RRTNode* node = nullptr;
 197                 KdNode* left = nullptr;
 198                 KdNode* right = nullptr;
 199                 KdNode(RRTNode* n);
 200         };
 201         KdNode* kdroot_ = nullptr;
 202         std::vector<KdNode> _kdnodes;
 203         void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
 204         void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
 205         void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
 206 public:
 207         RRTExt8();
 208         void reset();
 209         void store(RRTNode n);
 210         void find_nn(RRTNode const& t);
 211         void find_nv(RRTNode const& t);
 212 };
 213
 214 /*! \brief Use cute_c2 library for collision detection.
 215  *
 216  * \ingroup ext-col
 217  * \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 218  */
 219 class RRTExt2 : public virtual RRTS {
 220 private:
 221         c2Poly c2_bc_;
 222         c2x c2x_bc_;
 223         std::vector<c2Poly> c2_obstacles_;
 224         bool collide(RRTNode const& n);
 225         bool collide_steered();
 226 public:
 227         RRTExt2();
 228         void json(Json::Value jvi);
 229         void reset();
 230 };
 231
 232 } // namespace rrts
 233 #endif /* RRTS_RRTEXT_H */