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                 RRTNode *(*sample)();
  46                 std::vector<RRTNode *> (*steer)(
  47                                 RRTNode *init,
  48                                 RRTNode *goal);
  49                 float (*cost)(RRTNode *init, RRTNode *goal);
  50                 bool next();
  51 };
  52
  53 class Kuwata2008: public RRTBase {
  54         public:
  55                 Kuwata2008(RRTNode *init, RRTNode *goal);
  56
  57                 // RRT framework
  58                 RRTNode *(*nn)(
  59 #if NNVERSION>1
  60                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  61 #else
  62                                 std::vector<RRTNode *> &nodes,
  63 #endif
  64                                 RRTNode *node,
  65                                 float (*cost)(RRTNode *, RRTNode *)
  66 #if NNVERSION>2
  67                                 , char tree
  68 #endif
  69                                 );
  70                 RRTNode *(*sample)();
  71                 std::vector<RRTNode *> (*steer)(
  72                                 RRTNode *init,
  73                                 RRTNode *goal);
  74                 float (*cost)(RRTNode *init, RRTNode *goal);
  75                 bool next();
  76 };
  77
  78 class Karaman2011: public RRTBase {
  79         protected:
  80                 bool connect(
  81                                 RRTNode *pn,
  82                                 RRTNode *ns,
  83                                 std::vector<RRTNode *> nvs);
  84                 bool rewire(std::vector<RRTNode *> nvs, RRTNode *ns);
  85         public:
  86                 Karaman2011(RRTNode *init, RRTNode *goal);
  87
  88                 // RRT framework
  89                 RRTNode *(*nn)(
  90 #if NNVERSION>1
  91                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  92 #else
  93                                 std::vector<RRTNode *> &nodes,
  94 #endif
  95                                 RRTNode *node,
  96                                 float (*cost)(RRTNode *, RRTNode *)
  97 #if NNVERSION>2
  98                                 , char tree
  99 #endif
 100                                 );
 101                 std::vector<RRTNode *> (*nv)(
 102 #if NVVERSION>1
 103                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 104 #else
 105                                 RRTNode *root,
 106 #endif
 107                                 RRTNode *node,
 108                                 float (*cost)(RRTNode *, RRTNode *),
 109                                 float dist
 110 #if NVVERSION>2
 111                                 , char tree
 112 #endif
 113                                 );
 114                 RRTNode *(*sample)();
 115                 std::vector<RRTNode *> (*steer)(
 116                                 RRTNode *init,
 117                                 RRTNode *goal);
 118                 float (*cost)(RRTNode *init, RRTNode *goal);
 119                 bool next();
 120 };
 121
 122 class T1: public RRTBase {
 123         public:
 124                 T1(RRTNode *init, RRTNode *goal);
 125
 126                 // RRT framework
 127                 RRTNode *(*nn)(
 128 #if NNVERSION>1
 129                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 130 #else
 131                                 std::vector<RRTNode *> &nodes,
 132 #endif
 133                                 RRTNode *node,
 134                                 float (*cost)(RRTNode *, RRTNode *)
 135 #if NNVERSION>2
 136                                 , char tree
 137 #endif
 138                                 );
 139                 std::vector<RRTNode *> (*nv)(
 140 #if NVVERSION>1
 141                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 142 #else
 143                                 RRTNode *root,
 144 #endif
 145                                 RRTNode *node,
 146                                 float (*cost)(RRTNode *, RRTNode *),
 147                                 float dist
 148 #if NVVERSION>2
 149                                 , char tree
 150 #endif
 151                                 );
 152                 RRTNode *(*sample)();
 153                 std::vector<RRTNode *> (*steer)(
 154                                 RRTNode *init,
 155                                 RRTNode *goal);
 156                 float (*cost)(RRTNode *init, RRTNode *goal);
 157                 bool next();
 158 };
 159
 160 class T2: public Karaman2011 {
 161         public:
 162                 using Karaman2011::Karaman2011;
 163
 164                 bool next();
 165                 float goal_cost();
 166 };
 167
 168 class T3: public RRTBase {
 169         protected:
 170                 T2 p_root_;
 171                 T2 p_goal_;
 172         public:
 173                 T3(RRTNode *init, RRTNode *goal);
 174                 bool next();
 175                 bool link_obstacles(
 176                                 std::vector<CircleObstacle> *cobstacles,
 177                                 std::vector<SegmentObstacle> *sobstacles);
 178                 bool connecttrees(RRTNode *rn, RRTNode *gn);
 179                 bool overlaptrees(RRTNode **ron, RRTNode **gon);
 180 };
 181
 182 class Klemm2015: public Karaman2011 {
 183         private:
 184                 RRTNode *orig_root_ = nullptr;
 185                 RRTNode *orig_goal_ = nullptr;
 186         protected:
 187                 int extendstar();
 188                 int connectstar();
 189                 void swap();
 190         public:
 191                 Klemm2015(RRTNode *init, RRTNode *goal);
 192
 193                 // RRT framework
 194                 RRTNode *(*nn)(
 195 #if NNVERSION>1
 196                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 197 #else
 198                                 std::vector<RRTNode *> &nodes,
 199 #endif
 200                                 RRTNode *node,
 201                                 float (*cost)(RRTNode *, RRTNode *)
 202 #if NNVERSION>2
 203                                 , char tree
 204 #endif
 205                                 );
 206                 std::vector<RRTNode *> (*nv)(
 207 #if NVVERSION>1
 208                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 209 #else
 210                                 RRTNode *root,
 211 #endif
 212                                 RRTNode *node,
 213                                 float (*cost)(RRTNode *, RRTNode *),
 214                                 float dist
 215 #if NVVERSION>2
 216                                 , char tree
 217 #endif
 218                                 );
 219                 RRTNode *(*sample)();
 220                 std::vector<RRTNode *> (*steer)(
 221                                 RRTNode *init,
 222                                 RRTNode *goal);
 223                 float (*cost)(RRTNode *init, RRTNode *goal);
 224                 bool next();
 225 };
 226
 227 #endif