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