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 <pthread.h>
  24 #include <queue>
  25 #include <random>
  26 #include <vector>
  27 #include "obstacle.h"
  28 #include "rrtnode.h"
  29 #include "sample.h"
  30 #include "slotplanner.h"
  31
  32 #define NOFNODES 20000
  33
  34 #define IXSIZE 100
  35 #define IXSTEP (1.0 * ((VMAX) - (VMIN)) / IXSIZE)
  36 #define IXI(x) ({ __typeof__ (x) _x = (x); \
  37                 std::abs((int) floor(_x / IXSTEP)); })
  38 #define IYSIZE 100
  39 #define IYSTEP (1.0 * ((HMAX) - (HMIN)) / IYSIZE)
  40 #define IYI(y) ({ __typeof__ (y) _y = (y); \
  41                 std::abs((int) floor(_y / IYSTEP)); })
  42
  43 class Cell {
  44         private:
  45                 std::vector<RRTNode *> nodes_;
  46                 pthread_mutex_t m_;
  47                 bool changed_ = false;
  48         public:
  49                 Cell();
  50
  51                 // getter
  52                 bool changed();
  53                 std::vector<RRTNode *> nodes();
  54
  55                 // other
  56                 void add_node(RRTNode *n);
  57 };
  58
  59 class RRTBase {
  60         private:
  61                 RRTNode *root_ = nullptr;
  62                 RRTNode *goal_ = nullptr;
  63                 std::vector<RRTNode *> goals_;
  64
  65                 std::vector<RRTNode *> nodes_;
  66                 std::vector<RRTNode *> dnodes_;
  67                 std::queue<RRTNode *> firsts_;
  68                 PolygonObstacle frame_;
  69                 std::vector<RRTNode *> samples_;
  70                 std::vector<CircleObstacle> *cobstacles_;
  71                 std::vector<SegmentObstacle> *sobstacles_;
  72
  73                 bool goal_found_ = false;
  74                 std::chrono::high_resolution_clock::time_point tstart_;
  75                 std::chrono::high_resolution_clock::time_point tend_;
  76
  77                 std::vector<float> clog_; // costs of trajectories
  78                 std::vector<float> nlog_; // #nodes of RRT
  79                 std::vector<std::vector<RRTEdge *>> rlog_; // log tree
  80                 std::vector<float> slog_; // seconds of trajectories
  81                 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
  82                 std::vector<RRTNode *> slot_cusp_; // cusp nodes in slot
  83         protected:
  84                 std::default_random_engine gen_;
  85                 std::normal_distribution<float> ndx_;
  86                 std::normal_distribution<float> ndy_;
  87                 std::normal_distribution<float> ndh_;
  88         public:
  89                 const float GOAL_FOUND_DISTANCE = 0.2;
  90                 const float GOAL_FOUND_ANGLE = M_PI / 32;
  91
  92                 ~RRTBase();
  93                 RRTBase();
  94                 RRTBase(RRTNode *init, RRTNode *goal);
  95
  96                 // getter
  97                 RRTNode *root();
  98                 RRTNode *goal();
  99                 std::vector<RRTNode *> &goals();
 100                 std::vector<RRTNode *> &nodes();
 101                 std::vector<RRTNode *> &dnodes();
 102                 std::queue<RRTNode *> &firsts();
 103                 PolygonObstacle &frame();
 104                 std::vector<RRTNode *> &samples();
 105                 std::vector<RRTNode *> iy_[IYSIZE];
 106                 Cell ixy_[IXSIZE][IYSIZE];
 107                 std::vector<CircleObstacle> *co();
 108                 std::vector<SegmentObstacle> *so();
 109                 std::vector<float> &clog();
 110                 std::vector<float> &nlog();
 111                 std::vector<std::vector<RRTEdge *>> &rlog();
 112                 std::vector<float> &slog();
 113                 std::vector<std::vector<RRTNode *>> &tlog();
 114                 std::vector<RRTNode *> &slot_cusp();
 115                 bool goal_found();
 116                 float elapsed();
 117                 std::vector<RRTNode *> traj_cusp();
 118
 119                 // setter
 120                 void root(RRTNode *node);
 121                 void goal(RRTNode *node);
 122                 void goals(std::vector<RRTNode *> g);
 123                 bool logr(RRTNode *root);
 124                 float ocost(RRTNode *n);
 125                 bool tlog(std::vector<RRTNode *> t);
 126                 void tstart();
 127                 void tend();
 128                 bool link_obstacles(
 129                                 std::vector<CircleObstacle> *cobstacles,
 130                                 std::vector<SegmentObstacle> *sobstacles);
 131                 bool add_iy(RRTNode *n);
 132                 bool add_ixy(RRTNode *n);
 133                 bool goal_found(bool f);
 134                 void slot_cusp(std::vector<RRTNode *> sc);
 135
 136                 // other
 137                 bool glplot();
 138                 bool goal_found(
 139                                 RRTNode *node,
 140                                 float (*cost)(RRTNode *, RRTNode *));
 141                 bool goal_found(
 142                         RRTNode *node,
 143                         RRTNode *goal
 144                 );
 145                 bool collide(RRTNode *init, RRTNode *goal);
 146                 bool optp_dijkstra(
 147                                 std::vector<RRTNode *> &cusps,
 148                                 std::vector<int> &npi);
 149                 bool optp_rrp(
 150                                 std::vector<RRTNode *> &cusps,
 151                                 std::vector<int> &npi);
 152                 bool optp_smart(
 153                                 std::vector<RRTNode *> &cusps,
 154                                 std::vector<int> &npi);
 155                 bool opt_path();
 156                 bool rebase(RRTNode *nr);
 157                 std::vector<RRTNode *> findt();
 158                 std::vector<RRTNode *> findt(RRTNode *n);
 159
 160                 // RRT Framework
 161                 void setSamplingInfo(SamplingInfo si);
 162                 RRTNode *sample();
 163                 float cost(RRTNode *init, RRTNode *goal);
 164                 RRTNode *nn(RRTNode *rs);
 165                 std::vector<RRTNode *> nv(RRTNode *node, float dist);
 166                 std::vector<RRTNode *> steer(RRTNode *init, RRTNode *goal);
 167                 std::vector<RRTNode *> steer(
 168                                 RRTNode *init,
 169                                 RRTNode *goal,
 170                                 float step);
 171
 172                 // virtuals - implemented by child classes
 173                 virtual bool next() = 0;
 174 };
 175
 176 #endif