incl/rrtplanner.h

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #ifndef RRTPLANNER_H
  19 #define RRTPLANNER_H
  20
  21 #include <vector>
  22 #include "rrtbase.h"
  23 #include "rrtnode.h"
  24
  25 #define GOALFIRST 1
  26
  27 class LaValle1998: public RRTBase {
  28         public:
  29                 //using RRTBase::RRTBase;
  30                 LaValle1998(RRTNode *init, RRTNode *goal);
  31
  32                 // RRT framework
  33                 RRTNode *(*nn)(
  34 #if NNVERSION>1
  35                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  36 #else
  37                                 std::vector<RRTNode *> &nodes,
  38 #endif
  39                                 RRTNode *node,
  40                                 float (*cost)(RRTNode *, RRTNode *)
  41 #if NNVERSION>2
  42                                 , char tree
  43 #endif
  44                                 );
  45                 std::vector<RRTNode *> (*steer)(
  46                                 RRTNode *init,
  47                                 RRTNode *goal);
  48                 float (*cost)(RRTNode *init, RRTNode *goal);
  49                 bool next();
  50 };
  51
  52 class Kuwata2008: public RRTBase {
  53         public:
  54                 Kuwata2008(RRTNode *init, RRTNode *goal);
  55
  56                 // RRT framework
  57                 RRTNode *(*nn)(
  58 #if NNVERSION>1
  59                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  60 #else
  61                                 std::vector<RRTNode *> &nodes,
  62 #endif
  63                                 RRTNode *node,
  64                                 float (*cost)(RRTNode *, RRTNode *)
  65 #if NNVERSION>2
  66                                 , char tree
  67 #endif
  68                                 );
  69                 std::vector<RRTNode *> (*steer)(
  70                                 RRTNode *init,
  71                                 RRTNode *goal);
  72                 float (*cost)(RRTNode *init, RRTNode *goal);
  73                 bool next();
  74 };
  75
  76 class Karaman2011: public RRTBase {
  77         protected:
  78                 bool connect(
  79                                 RRTNode *pn,
  80                                 RRTNode *ns,
  81                                 std::vector<RRTNode *> nvs);
  82                 bool rewire(std::vector<RRTNode *> nvs, RRTNode *ns);
  83         public:
  84                 Karaman2011(RRTNode *init, RRTNode *goal);
  85
  86                 // RRT framework
  87                 RRTNode *(*nn)(
  88 #if NNVERSION>1
  89                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  90 #else
  91                                 std::vector<RRTNode *> &nodes,
  92 #endif
  93                                 RRTNode *node,
  94                                 float (*cost)(RRTNode *, RRTNode *)
  95 #if NNVERSION>2
  96                                 , char tree
  97 #endif
  98                                 );
  99                 std::vector<RRTNode *> (*nv)(
 100 #if NVVERSION>1
 101                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 102 #else
 103                                 RRTNode *root,
 104 #endif
 105                                 RRTNode *node,
 106                                 float (*cost)(RRTNode *, RRTNode *),
 107                                 float dist
 108 #if NVVERSION>2
 109                                 , char tree
 110 #endif
 111                                 );
 112                 std::vector<RRTNode *> (*steer)(
 113                                 RRTNode *init,
 114                                 RRTNode *goal);
 115                 float (*cost)(RRTNode *init, RRTNode *goal);
 116                 bool next();
 117 };
 118
 119 class T1: public RRTBase {
 120         public:
 121                 T1(RRTNode *init, RRTNode *goal);
 122
 123                 // RRT framework
 124                 RRTNode *(*nn)(
 125 #if NNVERSION>1
 126                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 127 #else
 128                                 std::vector<RRTNode *> &nodes,
 129 #endif
 130                                 RRTNode *node,
 131                                 float (*cost)(RRTNode *, RRTNode *)
 132 #if NNVERSION>2
 133                                 , char tree
 134 #endif
 135                                 );
 136                 std::vector<RRTNode *> (*nv)(
 137 #if NVVERSION>1
 138                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 139 #else
 140                                 RRTNode *root,
 141 #endif
 142                                 RRTNode *node,
 143                                 float (*cost)(RRTNode *, RRTNode *),
 144                                 float dist
 145 #if NVVERSION>2
 146                                 , char tree
 147 #endif
 148                                 );
 149                 std::vector<RRTNode *> (*steer)(
 150                                 RRTNode *init,
 151                                 RRTNode *goal);
 152                 float (*cost)(RRTNode *init, RRTNode *goal);
 153                 bool next();
 154 };
 155
 156 class T2: public Karaman2011 {
 157         public:
 158                 using Karaman2011::Karaman2011;
 159
 160                 bool next();
 161                 float goal_cost();
 162 };
 163
 164 class T3: public RRTBase {
 165         protected:
 166                 T2 p_root_;
 167                 T2 p_goal_;
 168         public:
 169                 T3(RRTNode *init, RRTNode *goal);
 170                 bool next();
 171                 bool link_obstacles(
 172                                 std::vector<CircleObstacle> *cobstacles,
 173                                 std::vector<SegmentObstacle> *sobstacles);
 174                 bool connecttrees(RRTNode *rn, RRTNode *gn);
 175                 bool overlaptrees(RRTNode **ron, RRTNode **gon);
 176 };
 177
 178 class Klemm2015: public Karaman2011 {
 179         private:
 180                 RRTNode *orig_root_ = nullptr;
 181                 RRTNode *orig_goal_ = nullptr;
 182         protected:
 183                 int extendstar1(RRTNode *rs, RRTNode **xn);
 184                 int extendstarC(RRTNode *rs);
 185                 int connectstar(RRTNode *x);
 186                 void swap();
 187         public:
 188                 Klemm2015(RRTNode *init, RRTNode *goal);
 189
 190                 // RRT framework
 191                 RRTNode *(*nn)(
 192 #if NNVERSION>1
 193                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 194 #else
 195                                 std::vector<RRTNode *> &nodes,
 196 #endif
 197                                 RRTNode *node,
 198                                 float (*cost)(RRTNode *, RRTNode *)
 199 #if NNVERSION>2
 200                                 , char tree
 201 #endif
 202                                 );
 203                 std::vector<RRTNode *> (*nv)(
 204 #if NVVERSION>1
 205                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 206 #else
 207                                 RRTNode *root,
 208 #endif
 209                                 RRTNode *node,
 210                                 float (*cost)(RRTNode *, RRTNode *),
 211                                 float dist
 212 #if NVVERSION>2
 213                                 , char tree
 214 #endif
 215                                 );
 216                 std::vector<RRTNode *> (*steer)(
 217                                 RRTNode *init,
 218                                 RRTNode *goal);
 219                 float (*cost)(RRTNode *init, RRTNode *goal);
 220                 bool next();
 221 };
 222
 223 #endif