api/rrtext.h

   1 #ifndef RRTEXT_H
   2 #define RRTEXT_H
   3
   4 #include "rrts.h"
   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 /*! \brief Different `steer` procedures.
  20
  21 Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
  22 */
  23 class RRTExt12 : public virtual RRTS {
  24         protected:
  25                 void steer1(RRTNode &f, RRTNode &t);
  26                 bool connect();
  27         public:
  28                 bool next();
  29 };
  30
  31 /*! \brief Goal Zone.
  32
  33 */
  34 class RRTExt11 : public virtual RRTS {
  35         protected:
  36                 bool goal_found(RRTNode &f);
  37 };
  38
  39 /*! \brief Different costs extension.
  40
  41 Use different cost for bulding tree data structure and searching in the
  42 structure. The cost function is from Elbanhawi, Mohamed, Milan Simic, and Reza
  43 Jazar. “Randomized Bidirectional B-Spline Parameterization Motion Planning.”
  44 IEEE Transactions on Intelligent Transportation Systems 17, no. 2 (February
  45 2016): 406–19. https://doi.org/10.1109/TITS.2015.2477355.
  46
  47 */
  48 class RRTExt10 : public virtual RRTS {
  49         public:
  50                 /*! \brief Reeds and Shepp path length.
  51                 */
  52                 double cost_build(RRTNode &f, RRTNode &t);
  53                 /*! \brief Heuristics distance.
  54                 */
  55                 double cost_search(RRTNode &f, RRTNode &t);
  56 };
  57
  58 /*! \brief Use grid data structure to store RRT nodes.
  59
  60 This approach speeds up the search process for the nearest neighbor and
  61 the near vertices procedures.
  62 */
  63 class RRTExt9 : public virtual RRTS {
  64         private:
  65                 class Cell {
  66                         private:
  67                                 bool changed_ = false;
  68                                 std::vector<RRTNode *> nodes_;
  69                         public:
  70                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
  71                                 void store_node(RRTNode *n);
  72
  73                                 // getter, setter
  74                                 bool changed() const
  75                                 {
  76                                         return this->changed_;
  77                                 }
  78                                 std::vector<RRTNode *> &nodes()
  79                                 {
  80                                         return this->nodes_;
  81                                 }
  82
  83                                 Cell();
  84                 };
  85                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI];
  86                 double x_min_ = 0;
  87                 double x_max_ = 0;
  88                 double y_min_ = 0;
  89                 double y_max_ = 0;
  90                 double h_min_ = 0;
  91                 double h_max_ = 2 * M_PI;
  92
  93                 unsigned int xi(RRTNode n);
  94                 unsigned int yi(RRTNode n);
  95                 unsigned int hi(RRTNode n);
  96         public:
  97                 void init();
  98                 void deinit();
  99                 void store_node(RRTNode n);
 100                 RRTNode *nn(RRTNode &t);
 101                 std::vector<RRTNode *> nv(RRTNode &t);
 102 };
 103
 104 /*! \brief Use k-d tree data structure to store RRT nodes.
 105
 106 This approach speeds up the search process for the nearest neighbor and
 107 the near vertices procedures. This extension implements 3D K-d tree.
 108
 109 \see https://en.wikipedia.org/wiki/K-d_tree
 110 */
 111 class RRTExt8 : public virtual RRTS {
 112         private:
 113                 class KdNode {
 114                         private:
 115                                 RRTNode *node_ = nullptr;
 116                                 KdNode *left_ = nullptr;
 117                                 KdNode *right_ = nullptr;
 118                         public:
 119                                 // getter, setter
 120                                 RRTNode *node() const { return this->node_; }
 121                                 KdNode *&left() { return this->left_; }
 122                                 KdNode *&right() { return this->right_; }
 123                                 KdNode(RRTNode *n);
 124                 };
 125                 KdNode *kdroot_ = nullptr;
 126                 void delete_kd_nodes(KdNode *n);
 127                 void store_node(RRTNode *n, KdNode *&r, int l);
 128                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
 129         public:
 130                 void init();
 131                 void deinit();
 132                 void store_node(RRTNode n);
 133                 RRTNode *nn(RRTNode &t);
 134                 std::vector<RRTNode *> nv(RRTNode &t);
 135 };
 136
 137 /*! \brief Use k-d tree data structure to store RRT nodes.
 138
 139 This approach speeds up the search process for the nearest neighbor and
 140 the near vertices procedures. This extension implements 2D K-d tree.
 141
 142 \see https://en.wikipedia.org/wiki/K-d_tree
 143 */
 144 class RRTExt7 : public virtual RRTS {
 145         private:
 146                 class KdNode {
 147                         private:
 148                                 RRTNode *node_ = nullptr;
 149                                 KdNode *left_ = nullptr;
 150                                 KdNode *right_ = nullptr;
 151                         public:
 152                                 // getter, setter
 153                                 RRTNode *node() const { return this->node_; }
 154                                 KdNode *&left() { return this->left_; }
 155                                 KdNode *&right() { return this->right_; }
 156                                 KdNode(RRTNode *n);
 157                 };
 158                 KdNode *kdroot_ = nullptr;
 159                 void delete_kd_nodes(KdNode *n);
 160                 void store_node(RRTNode *n, KdNode *&r, int l);
 161                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
 162         public:
 163                 void init();
 164                 void deinit();
 165                 void store_node(RRTNode n);
 166                 RRTNode *nn(RRTNode &t);
 167                 std::vector<RRTNode *> nv(RRTNode &t);
 168 };
 169
 170 /*! \brief Reeds and Shepp cost for building and search.
 171 */
 172 class RRTExt6 : public virtual RRTS {
 173         public:
 174                 /*! \brief Reeds and Shepp path length.
 175                 */
 176                 double cost_build(RRTNode &f, RRTNode &t);
 177                 /*! \brief Reeds and Shepp path length.
 178                 */
 179                 double cost_search(RRTNode &f, RRTNode &t);
 180 };
 181
 182 /*! \brief Different costs extension.
 183
 184 Use different cost for bulding tree data structure and searching in the
 185 structure. This one is complementary to `rrtext1.cc`.
 186 */
 187 class RRTExt5 : public virtual RRTS {
 188         public:
 189                 /*! \brief Reeds and Shepp path length.
 190                 */
 191                 double cost_build(RRTNode &f, RRTNode &t);
 192                 /*! \brief Euclidean distance.
 193                 */
 194                 double cost_search(RRTNode &f, RRTNode &t);
 195 };
 196
 197 /*! \brief Use grid data structure to store RRT nodes.
 198
 199 This approach speeds up the search process for the nearest neighbor and
 200 the near vertices procedures.
 201 */
 202 class RRTExt4 : public virtual RRTS {
 203         private:
 204                 class Cell {
 205                         private:
 206                                 bool changed_ = false;
 207                                 std::vector<RRTNode *> nodes_;
 208                         public:
 209                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
 210                                 void store_node(RRTNode *n);
 211
 212                                 // getter, setter
 213                                 bool changed() const
 214                                 {
 215                                         return this->changed_;
 216                                 }
 217                                 std::vector<RRTNode *> &nodes()
 218                                 {
 219                                         return this->nodes_;
 220                                 }
 221
 222                                 Cell();
 223                 };
 224                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
 225                 double x_min_ = 0;
 226                 double x_max_ = 0;
 227                 double y_min_ = 0;
 228                 double y_max_ = 0;
 229
 230                 unsigned int xi(RRTNode n);
 231                 unsigned int yi(RRTNode n);
 232         public:
 233                 void init();
 234                 void deinit();
 235                 void store_node(RRTNode n);
 236                 RRTNode *nn(RRTNode &t);
 237                 std::vector<RRTNode *> nv(RRTNode &t);
 238 };
 239
 240 /*! \brief Use Dijkstra algorithm to find the shorter path.
 241 */
 242 class RRTExt3 : public virtual RRTS {
 243         private:
 244                 std::vector<RRTNode *> orig_path_;
 245                 double orig_path_cost_ = 9999;
 246                 std::vector<RRTNode *> first_optimized_path_;
 247                 double first_optimized_path_cost_ = 9999;
 248         public:
 249                 std::vector<RRTNode *> first_path_optimization();
 250                 std::vector<RRTNode *> second_path_optimization();
 251                 std::vector<RRTNode *> path();
 252                 Json::Value json();
 253                 void json(Json::Value jvi);
 254
 255                 // getter, setter
 256                 std::vector<RRTNode *> &orig_path()
 257                 {
 258                         return this->orig_path_;
 259                 };
 260                 double &orig_path_cost() { return this->orig_path_cost_; }
 261                 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
 262                 std::vector<RRTNode *> &first_optimized_path()
 263                 {
 264                         return this->first_optimized_path_;
 265                 };
 266                 double &first_optimized_path_cost() {
 267                         return this->first_optimized_path_cost_;
 268                 }
 269                 void first_optimized_path_cost(double c) {
 270                         this->first_optimized_path_cost_ = c;
 271                 }
 272 };
 273
 274 /*! \brief Use cute_c2 for collision detection.
 275
 276 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 277 */
 278 class RRTExt2 : public virtual RRTS {
 279         private:
 280                 c2Poly c2_bc_;
 281                 c2x c2x_bc_;
 282                 std::vector<c2Poly> c2_obstacles_;
 283         public:
 284                 void init();
 285                 void deinit();
 286
 287                 // Collide RRT procedures
 288                 std::tuple<bool, unsigned int, unsigned int>
 289                 collide_steered_from(RRTNode &f);
 290                 std::tuple<bool, unsigned int, unsigned int>
 291                 collide_tmp_steered_from(RRTNode &f);
 292
 293                 std::tuple<bool, unsigned int, unsigned int>
 294                 collide_two_nodes(RRTNode &f, RRTNode &t);
 295
 296                 // getters, setters
 297                 c2Poly &c2_bc() { return this->c2_bc_; }
 298                 c2x &c2x_bc() { return this->c2x_bc_; }
 299                 std::vector<c2Poly> &c2_obstacles() {
 300                         return this->c2_obstacles_;
 301                 };
 302 };
 303
 304 /*! \brief Different costs extension.
 305
 306 Use different cost for bulding tree data structure and searching in the
 307 structure.
 308 */
 309 class RRTExt1 : public virtual RRTS {
 310         public:
 311                 /*! \brief Reeds and Shepp path length.
 312                 */
 313                 double cost_build(RRTNode &f, RRTNode &t);
 314                 /*! \brief Matej's heuristics.
 315                 */
 316                 double cost_search(RRTNode &f, RRTNode &t);
 317 };
 318
 319 #endif /* RRTEXT_H */