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                 RRTNode *(*sample)();
  42                 std::vector<RRTNode *> (*steer)(
  43                                 RRTNode *init,
  44                                 RRTNode *goal);
  45                 float (*cost)(RRTNode *init, RRTNode *goal);
  46                 bool next();
  47 };
  48
  49 class Kuwata2008: public RRTBase {
  50         public:
  51                 Kuwata2008(RRTNode *init, RRTNode *goal);
  52
  53                 // RRT framework
  54                 RRTNode *(*nn)(
  55 #if NNVERSION>1
  56                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  57 #else
  58                                 std::vector<RRTNode *> &nodes,
  59 #endif
  60                                 RRTNode *node,
  61                                 float (*cost)(RRTNode *, RRTNode *));
  62                 RRTNode *(*sample)();
  63                 std::vector<RRTNode *> (*steer)(
  64                                 RRTNode *init,
  65                                 RRTNode *goal);
  66                 float (*cost)(RRTNode *init, RRTNode *goal);
  67                 bool next();
  68 };
  69
  70 class Karaman2011: public RRTBase {
  71         protected:
  72                 bool connect(
  73                                 RRTNode *pn,
  74                                 RRTNode *ns,
  75                                 std::vector<RRTNode *> nvs);
  76                 bool rewire(std::vector<RRTNode *> nvs, RRTNode *ns);
  77         public:
  78                 Karaman2011(RRTNode *init, RRTNode *goal);
  79
  80                 // RRT framework
  81                 RRTNode *(*nn)(
  82 #if NNVERSION>1
  83                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  84 #else
  85                                 std::vector<RRTNode *> &nodes,
  86 #endif
  87                                 RRTNode *node,
  88                                 float (*cost)(RRTNode *, RRTNode *));
  89                 std::vector<RRTNode *> (*nv)(
  90 #if NVVERSION>1
  91                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
  92 #else
  93                                 RRTNode *root,
  94 #endif
  95                                 RRTNode *node,
  96                                 float (*cost)(RRTNode *, RRTNode *),
  97                                 float dist);
  98                 RRTNode *(*sample)();
  99                 std::vector<RRTNode *> (*steer)(
 100                                 RRTNode *init,
 101                                 RRTNode *goal);
 102                 float (*cost)(RRTNode *init, RRTNode *goal);
 103                 bool next();
 104 };
 105
 106 class T1: public RRTBase {
 107         public:
 108                 T1(RRTNode *init, RRTNode *goal);
 109
 110                 // RRT framework
 111                 RRTNode *(*nn)(
 112 #if NNVERSION>1
 113                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 114 #else
 115                                 std::vector<RRTNode *> &nodes,
 116 #endif
 117                                 RRTNode *node,
 118                                 float (*cost)(RRTNode *, RRTNode *));
 119                 std::vector<RRTNode *> (*nv)(
 120 #if NVVERSION>1
 121                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
 122 #else
 123                                 RRTNode *root,
 124 #endif
 125                                 RRTNode *node,
 126                                 float (*cost)(RRTNode *, RRTNode *),
 127                                 float dist);
 128                 RRTNode *(*sample)();
 129                 std::vector<RRTNode *> (*steer)(
 130                                 RRTNode *init,
 131                                 RRTNode *goal);
 132                 float (*cost)(RRTNode *init, RRTNode *goal);
 133                 bool next();
 134 };
 135
 136 class T2: public Karaman2011 {
 137         public:
 138                 using Karaman2011::Karaman2011;
 139
 140                 bool next();
 141                 float goal_cost();
 142                 bool opt_path();
 143                 bool opt_part(RRTNode *init, RRTNode *goal);
 144 };
 145
 146 #endif