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* node = nullptr;
 127                 unsigned int i = 0;
 128                 bool v = false;
 129                 double d = 0.0;
 130                 bool vi();
 131                 DijkstraNode(RRTNode* n);
 132         };
 133         class DijkstraNodeComparator {
 134         public:
 135                 int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
 136         };
 137         class DijkstraNodeBackwardComparator {
 138         public:
 139                 int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
 140         };
 141         std::vector<RRTNode*> opath_;
 142         double ogoal_cc_ = 0.0;
 143         double otime_ = 0.0;
 144         std::vector<DijkstraNode> dn_;
 145         void interesting_forward();
 146         void interesting_backward();
 147         void dijkstra_forward();
 148         void dijkstra_backward();
 149         void compute_path();
 150 public:
 151         RRTExt13();
 152         Json::Value json() const;
 153         void json(Json::Value jvi);
 154         void reset();
 155 };
 156
 157 /*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
 158  *
 159  * Use Reeds & Shepp path length for building tree data structure and Euclidean
 160  * distance + (abs) heading difference + 0.1 * backward-forward direction
 161  * changes for searching it.
 162  *
 163  * \ingroup ext-cost
 164  * \see https://doi.org/10.1109/TITS.2015.2477355
 165  */
 166 class RRTExt10 : public virtual RRTS {
 167 protected:
 168         double cost_build(RRTNode const& f, RRTNode const& t) const;
 169         double cost_search(RRTNode const& f, RRTNode const& t) const;
 170 };
 171
 172 /*! \brief Use 3D k-d tree data structure to store RRT nodes.
 173  *
 174  * This approach speeds up the search process for the nearest neighbor and the
 175  * near vertices procedures. This extension implements 3D K-d tree.
 176  *
 177  * \ingroup ext-store
 178  * \see https://en.wikipedia.org/wiki/K-d_tree
 179  */
 180 class RRTExt8 : public virtual RRTS {
 181 private:
 182         class KdNode {
 183         public:
 184                 RRTNode* node = nullptr;
 185                 KdNode* left = nullptr;
 186                 KdNode* right = nullptr;
 187                 KdNode(RRTNode* n);
 188         };
 189         KdNode* kdroot_ = nullptr;
 190         std::vector<KdNode> _kdnodes;
 191         void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
 192         void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
 193         void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
 194 public:
 195         RRTExt8();
 196         void reset();
 197         void store(RRTNode n);
 198         void find_nn(RRTNode const& t);
 199         void find_nv(RRTNode const& t);
 200 };
 201
 202 /*! \brief Use cute_c2 library for collision detection.
 203  *
 204  * \ingroup ext-col
 205  * \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 206  */
 207 class RRTExt2 : public virtual RRTS {
 208 private:
 209         c2Poly c2_bc_;
 210         c2x c2x_bc_;
 211         std::vector<c2Poly> c2_obstacles_;
 212         bool collide(RRTNode const& n);
 213         bool collide_steered();
 214 public:
 215         RRTExt2();
 216         Json::Value json() const;
 217         void json(Json::Value jvi);
 218         void reset();
 219 };
 220
 221 } // namespace rrts
 222 #endif /* RRTS_RRTEXT_H */