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