incl/rrtext.hh

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