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 Use Dubins paths-based steering procedure.
  42  *
  43  * \ingroup ext-steer
  44  * \see https://github.com/AndrewWalker/Dubins-Curves
  45  */
  46 class RRTExt19 : public virtual RRTS {
  47 };
  48
  49 /*! \brief Finish when more than 1000 iterations.
  50  *
  51  * \ingroup ext-aux
  52  */
  53 class RRTExt18 : public virtual RRTS {
  54 private:
  55         bool should_finish() const;
  56 };
  57
  58 /*! \brief Finish when goal found or more than 1000 iterations.
  59  *
  60  * \ingroup ext-aux
  61  */
  62 class RRTExt17 : public virtual RRTS {
  63 private:
  64         bool should_finish() const;
  65 };
  66
  67 /*! \brief Use Reeds & Shepp steering procedure.
  68  *
  69  * \ingroup ext-steer
  70  */
  71 class RRTExt16 : public virtual RRTS {
  72 private:
  73         void steer(RRTNode const& f, RRTNode const& t);
  74 };
  75
  76 /*! \brief Log goal's cumulative cost each iteration.
  77  *
  78  * \ingroup ext-aux
  79  */
  80 class RRTExt15 : public virtual RRTS {
  81 private:
  82         std::vector<double> log_path_cost_;
  83 public:
  84         Json::Value json() const;
  85         void json(Json::Value jvi);
  86         bool next();
  87 };
  88
  89 /*! \brief Random sampling in the circuit between root and goal.
  90  *
  91  * \ingroup ext-sampl
  92  * \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
  93  */
  94 class RRTExt14 : public virtual RRTS {
  95 private:
  96         Point circle_c_;
  97         double circle_r_ = 0.0;
  98         std::uniform_real_distribution<double> udr_;
  99         std::uniform_real_distribution<double> udt_;
 100         std::uniform_real_distribution<double> udh_;
 101         RRTNode sample();
 102 public:
 103         RRTExt14();
 104         void reset();
 105 };
 106
 107 /*! \brief Use Dijkstra algorithm to find path between interesting nodes.
 108  *
 109  * The search for interesting nodes starts at root, the last drivable nodes is
 110  * added to interesting nodes until goal is reached.
 111  *
 112  * \ingroup ext-opt
 113  */
 114 class RRTExt13 : public virtual RRTS {
 115 private:
 116         class DijkstraNode {
 117         public:
 118                 RRTNode* node = nullptr;
 119                 unsigned int i = 0;
 120                 bool v = false;
 121                 double d = 0.0;
 122                 bool vi();
 123                 DijkstraNode(RRTNode* n);
 124         };
 125         class DijkstraNodeComparator {
 126         public:
 127                 int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
 128         };
 129         class DijkstraNodeBackwardComparator {
 130         public:
 131                 int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
 132         };
 133         std::vector<RRTNode*> opath_;
 134         double ogoal_cc_ = 0.0;
 135         double otime_ = 0.0;
 136         std::vector<DijkstraNode> dn_;
 137         void interesting_forward();
 138         void interesting_backward();
 139         void dijkstra_forward();
 140         void dijkstra_backward();
 141         void compute_path();
 142 public:
 143         RRTExt13();
 144         Json::Value json() const;
 145         void json(Json::Value jvi);
 146         void reset();
 147 };
 148
 149 /* \brief Different `steer` procedures.
 150
 151 Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
 152 */
 153 class RRTExt12 : public virtual RRTS {
 154         protected:
 155                 void steer1(RRTNode &f, RRTNode &t);
 156                 bool connect();
 157         public:
 158                 bool next();
 159 };
 160
 161 /* \brief Goal Zone.
 162
 163 */
 164 class RRTExt11 : public virtual RRTS {
 165         protected:
 166                 bool goal_found(RRTNode &f);
 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 grid data structure to store RRT nodes.
 185
 186 This approach speeds up the search process for the nearest neighbor and
 187 the near vertices procedures.
 188 */
 189 class RRTExt9 : public virtual RRTS {
 190         private:
 191                 class Cell {
 192                         private:
 193                                 bool changed_ = false;
 194                                 std::vector<RRTNode *> nodes_;
 195                         public:
 196                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
 197                                 void store_node(RRTNode *n);
 198
 199                                 // getter, setter
 200                                 bool changed() const
 201                                 {
 202                                         return this->changed_;
 203                                 }
 204                                 std::vector<RRTNode *> &nodes()
 205                                 {
 206                                         return this->nodes_;
 207                                 }
 208
 209                                 Cell();
 210                 };
 211                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI];
 212                 double x_min_ = 0;
 213                 double x_max_ = 0;
 214                 double y_min_ = 0;
 215                 double y_max_ = 0;
 216                 double h_min_ = 0;
 217                 double h_max_ = 2 * M_PI;
 218
 219                 unsigned int xi(RRTNode n);
 220                 unsigned int yi(RRTNode n);
 221                 unsigned int hi(RRTNode n);
 222         public:
 223                 void init();
 224                 void deinit();
 225                 void store_node(RRTNode n);
 226                 RRTNode *nn(RRTNode &t);
 227                 std::vector<RRTNode *> nv(RRTNode &t);
 228 };
 229
 230 /*! \brief Use 3D k-d tree data structure to store RRT nodes.
 231  *
 232  * This approach speeds up the search process for the nearest neighbor and the
 233  * near vertices procedures. This extension implements 3D K-d tree.
 234  *
 235  * \ingroup ext-store
 236  * \see https://en.wikipedia.org/wiki/K-d_tree
 237  */
 238 class RRTExt8 : public virtual RRTS {
 239 private:
 240         class KdNode {
 241         public:
 242                 RRTNode* node = nullptr;
 243                 KdNode* left = nullptr;
 244                 KdNode* right = nullptr;
 245                 KdNode(RRTNode* n);
 246         };
 247         KdNode* kdroot_ = nullptr;
 248         std::vector<KdNode> kdnodes_;
 249         void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
 250         void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
 251         void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
 252 public:
 253         RRTExt8();
 254         void reset();
 255         void store(RRTNode n);
 256         void find_nn(RRTNode const& t);
 257         void find_nv(RRTNode const& t);
 258 };
 259
 260 /* \brief Use k-d tree data structure to store RRT nodes.
 261
 262 This approach speeds up the search process for the nearest neighbor and
 263 the near vertices procedures. This extension implements 2D K-d tree.
 264
 265 \see https://en.wikipedia.org/wiki/K-d_tree
 266 */
 267 class RRTExt7 : public virtual RRTS {
 268         private:
 269                 class KdNode {
 270                         private:
 271                                 RRTNode *node_ = nullptr;
 272                                 KdNode *left_ = nullptr;
 273                                 KdNode *right_ = nullptr;
 274                         public:
 275                                 // getter, setter
 276                                 RRTNode *node() const { return this->node_; }
 277                                 KdNode *&left() { return this->left_; }
 278                                 KdNode *&right() { return this->right_; }
 279                                 KdNode(RRTNode *n);
 280                 };
 281                 KdNode *kdroot_ = nullptr;
 282                 void delete_kd_nodes(KdNode *n);
 283                 void store_node(RRTNode *n, KdNode *&r, int l);
 284                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
 285         public:
 286                 void init();
 287                 void deinit();
 288                 void store_node(RRTNode n);
 289                 RRTNode *nn(RRTNode &t);
 290                 std::vector<RRTNode *> nv(RRTNode &t);
 291 };
 292
 293 /*! \brief Reeds & Shepp (build, search).
 294  *
 295  * Use Reeds & Shepp path length for building tree data structure as well as for
 296  * searching it.
 297  *
 298  * \ingroup ext-cost
 299  */
 300 class RRTExt6 : public virtual RRTS {
 301 private:
 302         double cost_build(RRTNode const& f, RRTNode const& t) const;
 303         double cost_search(RRTNode const& f, RRTNode const& t) const;
 304 };
 305
 306 /* \brief Different costs extension.
 307
 308 Use different cost for bulding tree data structure and searching in the
 309 structure. This one is complementary to `rrtext1.cc`.
 310 */
 311 class RRTExt5 : public virtual RRTS {
 312         public:
 313                 /* \brief Reeds and Shepp path length.
 314                 */
 315                 double cost_build(RRTNode &f, RRTNode &t);
 316                 /* \brief Euclidean distance.
 317                 */
 318                 double cost_search(RRTNode &f, RRTNode &t);
 319 };
 320
 321 /* \brief Use grid data structure to store RRT nodes.
 322
 323 This approach speeds up the search process for the nearest neighbor and
 324 the near vertices procedures.
 325 */
 326 class RRTExt4 : public virtual RRTS {
 327         private:
 328                 class Cell {
 329                         private:
 330                                 bool changed_ = false;
 331                                 std::vector<RRTNode *> nodes_;
 332                         public:
 333                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
 334                                 void store_node(RRTNode *n);
 335
 336                                 // getter, setter
 337                                 bool changed() const
 338                                 {
 339                                         return this->changed_;
 340                                 }
 341                                 std::vector<RRTNode *> &nodes()
 342                                 {
 343                                         return this->nodes_;
 344                                 }
 345
 346                                 Cell();
 347                 };
 348                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
 349                 double x_min_ = 0;
 350                 double x_max_ = 0;
 351                 double y_min_ = 0;
 352                 double y_max_ = 0;
 353
 354                 unsigned int xi(RRTNode n);
 355                 unsigned int yi(RRTNode n);
 356         public:
 357                 void init();
 358                 void deinit();
 359                 void store_node(RRTNode n);
 360                 RRTNode *nn(RRTNode &t);
 361                 std::vector<RRTNode *> nv(RRTNode &t);
 362 };
 363
 364 /* \brief Use Dijkstra algorithm to find the shorter path.
 365 */
 366 class RRTExt3 : public virtual RRTS {
 367         private:
 368         public:
 369                 void reset();
 370                 std::vector<RRTNode *> orig_path_;
 371                 double orig_path_cost_ = 9999;
 372                 std::vector<RRTNode *> first_optimized_path_;
 373                 double first_optimized_path_cost_ = 9999;
 374                 void first_path_optimization();
 375                 void second_path_optimization();
 376                 void compute_path();
 377                 Json::Value json();
 378                 void json(Json::Value jvi);
 379
 380                 // getter, setter
 381                 std::vector<RRTNode *> &orig_path()
 382                 {
 383                         return this->orig_path_;
 384                 };
 385                 double &orig_path_cost() { return this->orig_path_cost_; }
 386                 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
 387                 std::vector<RRTNode *> &first_optimized_path()
 388                 {
 389                         return this->first_optimized_path_;
 390                 };
 391                 double &first_optimized_path_cost() {
 392                         return this->first_optimized_path_cost_;
 393                 }
 394                 void first_optimized_path_cost(double c) {
 395                         this->first_optimized_path_cost_ = c;
 396                 }
 397 };
 398
 399 /*! \brief Use cute_c2 library for collision detection.
 400  *
 401  * \ingroup ext-col
 402  * \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 403  */
 404 class RRTExt2 : public virtual RRTS {
 405 private:
 406         c2Poly c2_bc_;
 407         c2x c2x_bc_;
 408         std::vector<c2Poly> c2_obstacles_;
 409         bool collide(RRTNode const& n);
 410         bool collide_steered();
 411 public:
 412         RRTExt2();
 413         Json::Value json() const;
 414         void json(Json::Value jvi);
 415 };
 416
 417 /* \brief Different costs extension.
 418
 419 Use different cost for bulding tree data structure and searching in the
 420 structure.
 421 */
 422 class RRTExt1 : public virtual RRTS {
 423         public:
 424                 /* \brief Reeds and Shepp path length.
 425                 */
 426                 double cost_build(RRTNode &f, RRTNode &t);
 427                 /* \brief Matej's heuristics.
 428                 */
 429                 double cost_search(RRTNode &f, RRTNode &t);
 430 };
 431
 432 } // namespace rrts
 433 #endif /* RRTS_RRTEXT_H */