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         public:
  23                 std::vector<RRTNode *> orig_path_;
  24                 double orig_path_cost_ = 9999;
  25                 std::vector<RRTNode *> first_optimized_path_;
  26                 double first_optimized_path_cost_ = 9999;
  27                 void first_path_optimization();
  28                 void second_path_optimization();
  29                 void compute_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                 void nv(
 163                         std::vector<RRTNode*>& n,
 164                         RRTNode &t,
 165                         KdNode *r,
 166                         int l,
 167                         double const& d
 168                 );
 169         public:
 170                 void delete_kd_nodes()
 171                 {
 172                     this->delete_kd_nodes(this->kdroot_);
 173                     this->kdroot_ = nullptr;
 174                 }
 175                 void init();
 176                 void deinit();
 177                 void store_node(RRTNode n);
 178                 RRTNode *nn(RRTNode &t);
 179                 std::vector<RRTNode *> nv(RRTNode &t);
 180 };
 181
 182 /*! \brief Use k-d tree data structure to store RRT nodes.
 183
 184 This approach speeds up the search process for the nearest neighbor and
 185 the near vertices procedures. This extension implements 2D K-d tree.
 186
 187 \see https://en.wikipedia.org/wiki/K-d_tree
 188 */
 189 class RRTExt7 : public virtual RRTS {
 190         private:
 191                 class KdNode {
 192                         private:
 193                                 RRTNode *node_ = nullptr;
 194                                 KdNode *left_ = nullptr;
 195                                 KdNode *right_ = nullptr;
 196                         public:
 197                                 // getter, setter
 198                                 RRTNode *node() const { return this->node_; }
 199                                 KdNode *&left() { return this->left_; }
 200                                 KdNode *&right() { return this->right_; }
 201                                 KdNode(RRTNode *n);
 202                 };
 203                 KdNode *kdroot_ = nullptr;
 204                 void delete_kd_nodes(KdNode *n);
 205                 void store_node(RRTNode *n, KdNode *&r, int l);
 206                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
 207         public:
 208                 void init();
 209                 void deinit();
 210                 void store_node(RRTNode n);
 211                 RRTNode *nn(RRTNode &t);
 212                 std::vector<RRTNode *> nv(RRTNode &t);
 213 };
 214
 215 /*! \brief Reeds and Shepp cost for building and search.
 216 */
 217 class RRTExt6 : public virtual RRTS {
 218         public:
 219                 /*! \brief Reeds and Shepp path length.
 220                 */
 221                 double cost_build(RRTNode &f, RRTNode &t);
 222                 /*! \brief Reeds and Shepp path length.
 223                 */
 224                 double cost_search(RRTNode &f, RRTNode &t);
 225 };
 226
 227 /*! \brief Different costs extension.
 228
 229 Use different cost for bulding tree data structure and searching in the
 230 structure. This one is complementary to `rrtext1.cc`.
 231 */
 232 class RRTExt5 : public virtual RRTS {
 233         public:
 234                 /*! \brief Reeds and Shepp path length.
 235                 */
 236                 double cost_build(RRTNode &f, RRTNode &t);
 237                 /*! \brief Euclidean distance.
 238                 */
 239                 double cost_search(RRTNode &f, RRTNode &t);
 240 };
 241
 242 /*! \brief Use grid data structure to store RRT nodes.
 243
 244 This approach speeds up the search process for the nearest neighbor and
 245 the near vertices procedures.
 246 */
 247 class RRTExt4 : public virtual RRTS {
 248         private:
 249                 class Cell {
 250                         private:
 251                                 bool changed_ = false;
 252                                 std::vector<RRTNode *> nodes_;
 253                         public:
 254                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
 255                                 void store_node(RRTNode *n);
 256
 257                                 // getter, setter
 258                                 bool changed() const
 259                                 {
 260                                         return this->changed_;
 261                                 }
 262                                 std::vector<RRTNode *> &nodes()
 263                                 {
 264                                         return this->nodes_;
 265                                 }
 266
 267                                 Cell();
 268                 };
 269                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
 270                 double x_min_ = 0;
 271                 double x_max_ = 0;
 272                 double y_min_ = 0;
 273                 double y_max_ = 0;
 274
 275                 unsigned int xi(RRTNode n);
 276                 unsigned int yi(RRTNode n);
 277         public:
 278                 void init();
 279                 void deinit();
 280                 void store_node(RRTNode n);
 281                 RRTNode *nn(RRTNode &t);
 282                 std::vector<RRTNode *> nv(RRTNode &t);
 283 };
 284
 285 /*! \brief Use Dijkstra algorithm to find the shorter path.
 286 */
 287 class RRTExt3 : public virtual RRTS {
 288         private:
 289         public:
 290                 std::vector<RRTNode *> orig_path_;
 291                 double orig_path_cost_ = 9999;
 292                 std::vector<RRTNode *> first_optimized_path_;
 293                 double first_optimized_path_cost_ = 9999;
 294                 void first_path_optimization();
 295                 void second_path_optimization();
 296                 void compute_path();
 297                 Json::Value json();
 298                 void json(Json::Value jvi);
 299
 300                 // getter, setter
 301                 std::vector<RRTNode *> &orig_path()
 302                 {
 303                         return this->orig_path_;
 304                 };
 305                 double &orig_path_cost() { return this->orig_path_cost_; }
 306                 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
 307                 std::vector<RRTNode *> &first_optimized_path()
 308                 {
 309                         return this->first_optimized_path_;
 310                 };
 311                 double &first_optimized_path_cost() {
 312                         return this->first_optimized_path_cost_;
 313                 }
 314                 void first_optimized_path_cost(double c) {
 315                         this->first_optimized_path_cost_ = c;
 316                 }
 317 };
 318
 319 /*! \brief Use cute_c2 for collision detection.
 320
 321 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 322 */
 323 class RRTExt2 : public virtual RRTS {
 324         private:
 325                 c2Poly c2_bc_;
 326                 c2x c2x_bc_;
 327                 std::vector<c2Poly> c2_obstacles_;
 328         public:
 329                 void init();
 330                 void deinit();
 331
 332                 // Collide RRT procedures
 333                 std::tuple<bool, unsigned int, unsigned int>
 334                 collide_steered_from(RRTNode &f);
 335                 std::tuple<bool, unsigned int, unsigned int>
 336                 collide_tmp_steered_from(RRTNode &f);
 337
 338                 std::tuple<bool, unsigned int, unsigned int>
 339                 collide_two_nodes(RRTNode &f, RRTNode &t);
 340
 341                 // getters, setters
 342                 c2Poly &c2_bc() { return this->c2_bc_; }
 343                 c2x &c2x_bc() { return this->c2x_bc_; }
 344                 std::vector<c2Poly> &c2_obstacles() {
 345                         return this->c2_obstacles_;
 346                 };
 347 };
 348
 349 /*! \brief Different costs extension.
 350
 351 Use different cost for bulding tree data structure and searching in the
 352 structure.
 353 */
 354 class RRTExt1 : public virtual RRTS {
 355         public:
 356                 /*! \brief Reeds and Shepp path length.
 357                 */
 358                 double cost_build(RRTNode &f, RRTNode &t);
 359                 /*! \brief Matej's heuristics.
 360                 */
 361                 double cost_search(RRTNode &f, RRTNode &t);
 362 };
 363
 364 #endif /* RRTEXT_H */