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