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