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
  27 class RRTBase {
  28         private:
  29                 RRTNode *root_ = nullptr;
  30                 RRTNode *goal_ = nullptr;
  31
  32                 std::vector<RRTNode *> nodes_;
  33                 std::vector<RRTNode *> samples_;
  34                 std::vector<CircleObstacle> *cobstacles_;
  35                 std::vector<SegmentObstacle> *sobstacles_;
  36
  37                 bool goal_found_ = false;
  38                 std::chrono::high_resolution_clock::time_point tstart_;
  39                 std::chrono::high_resolution_clock::time_point tend_;
  40
  41                 std::vector<float> clog_; // costs of trajectories
  42                 std::vector<float> slog_; // seconds of trajectories
  43                 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
  44         public:
  45                 const float GOAL_FOUND_DISTANCE = 1;
  46                 const float GOAL_FOUND_ANGLE = 2 * M_PI;
  47
  48                 RRTBase();
  49                 RRTBase(RRTNode *init, RRTNode *goal);
  50
  51                 // getter
  52                 RRTNode *root();
  53                 RRTNode *goal();
  54                 std::vector<RRTNode *> &nodes();
  55                 std::vector<RRTNode *> &samples();
  56                 std::vector<CircleObstacle> *cos();
  57                 std::vector<SegmentObstacle> *sos();
  58                 std::vector<float> &clog();
  59                 std::vector<float> &slog();
  60                 std::vector<std::vector<RRTNode *>> &tlog();
  61                 bool goal_found();
  62                 float elapsed();
  63
  64                 // setter
  65                 bool tlog(std::vector<RRTNode *> t);
  66                 void tstart();
  67                 void tend();
  68                 bool link_obstacles(
  69                                 std::vector<CircleObstacle> *cobstacles,
  70                                 std::vector<SegmentObstacle> *sobstacles);
  71
  72                 // other
  73                 bool goal_found(
  74                                 RRTNode *node,
  75                                 float (*cost)(RRTNode *, RRTNode *));
  76                 bool collide(RRTNode *init, RRTNode *goal);
  77                 std::vector<RRTNode *> findt();
  78 };
  79
  80 #endif