incl/rrtext.hh

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