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