incl/rrtext.hh

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