incl/rrtbase.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 RRTBASE_H
  19 #define RRTBASE_H
  20
  21 #include <chrono>
  22 #include <cmath>
  23 #include <vector>
  24 #include "obstacle.h"
  25 #include "rrtnode.h"
  26 #include "sample.h"
  27
  28 #define IYSIZE 200
  29 #define IYSTEP (1.0 * ((HMAX) - (HMIN)) / IYSIZE)
  30 #define IYI(y) ({ __typeof__ (y) _y = (y); \
  31                 (int) floor(_y / IYSTEP); })
  32
  33 class RRTBase {
  34         private:
  35                 RRTNode *root_ = nullptr;
  36                 RRTNode *goal_ = nullptr;
  37
  38                 std::vector<RRTNode *> nodes_;
  39                 std::vector<RRTNode *> dnodes_;
  40                 std::vector<RRTNode *> samples_;
  41                 std::vector<CircleObstacle> *cobstacles_;
  42                 std::vector<SegmentObstacle> *sobstacles_;
  43
  44                 bool goal_found_ = false;
  45                 std::chrono::high_resolution_clock::time_point tstart_;
  46                 std::chrono::high_resolution_clock::time_point tend_;
  47
  48                 std::vector<float> clog_; // costs of trajectories
  49                 std::vector<float> nlog_; // #nodes of RRT
  50                 std::vector<std::vector<RRTEdge *>> rlog_; // log tree
  51                 std::vector<float> slog_; // seconds of trajectories
  52                 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
  53         public:
  54                 const float GOAL_FOUND_DISTANCE = 0.2;
  55                 const float GOAL_FOUND_ANGLE = M_PI / 32;
  56
  57                 ~RRTBase();
  58                 RRTBase();
  59                 RRTBase(RRTNode *init, RRTNode *goal);
  60
  61                 // getter
  62                 RRTNode *root();
  63                 RRTNode *goal();
  64                 std::vector<RRTNode *> &nodes();
  65                 std::vector<RRTNode *> &dnodes();
  66                 std::vector<RRTNode *> &samples();
  67                 std::vector<RRTNode *> iy_[IYSIZE];
  68                 std::vector<CircleObstacle> *cos();
  69                 std::vector<SegmentObstacle> *sos();
  70                 std::vector<float> &clog();
  71                 std::vector<float> &nlog();
  72                 std::vector<std::vector<RRTEdge *>> &rlog();
  73                 std::vector<float> &slog();
  74                 std::vector<std::vector<RRTNode *>> &tlog();
  75                 bool goal_found();
  76                 float elapsed();
  77
  78                 // setter
  79                 void root(RRTNode *node);
  80                 void goal(RRTNode *node);
  81                 bool logr(RRTNode *root);
  82                 float ocost(RRTNode *n);
  83                 bool tlog(std::vector<RRTNode *> t);
  84                 void tstart();
  85                 void tend();
  86                 bool link_obstacles(
  87                                 std::vector<CircleObstacle> *cobstacles,
  88                                 std::vector<SegmentObstacle> *sobstacles);
  89                 bool add_iy(RRTNode *n);
  90                 bool goal_found(bool f);
  91
  92                 // other
  93                 bool glplot();
  94                 bool goal_found(
  95                                 RRTNode *node,
  96                                 float (*cost)(RRTNode *, RRTNode *));
  97                 bool collide(RRTNode *init, RRTNode *goal);
  98                 bool opt_path();
  99                 bool rebase(RRTNode *nr);
 100                 std::vector<RRTNode *> findt();
 101                 std::vector<RRTNode *> findt(RRTNode *n);
 102
 103                 // RRT Framework
 104                 RRTNode *sample();
 105                 float cost(RRTNode *init, RRTNode *goal);
 106                 RRTNode *nn(RRTNode *rs);
 107
 108                 // virtuals - implemented by child classes
 109                 virtual bool next() = 0;
 110 };
 111
 112 #endif