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