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