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