api/rrts.h

   1 #ifndef RRTS_H
   2 #define RRTS_H
   3
   4 #include <chrono>
   5 #include <functional>
   6 #include <random>
   7 #include <vector>
   8 #include "bcar.h"
   9
  10 #define ETA 1.0 // for steer, nv
  11 #define GAMMA(cV) ({ \
  12         __typeof__ (cV) _cV = (cV); \
  13         pow(log(_cV) / _cV, 1.0 / 3.0); \
  14 })
  15
  16 /*! \brief Possible type of RRT node.
  17
  18 \param cusp The node that is cusp (change in direction).
  19 \param connected The node that branches generated steered path.
  20 */
  21 class RRTNodeType {
  22         public:
  23                 static const unsigned int cusp = 1 << 0;
  24                 static const unsigned int connected = 1 << 1;
  25 };
  26
  27 /*! \brief RRT node basic class.
  28
  29 \param c Cumulative cost from RRT data structure root.
  30 \param p Pointer to parent RRT node.
  31 \param ch The vector of pointers to children RRT nodes.
  32 */
  33 class RRTNode : public BicycleCar {
  34         private:
  35                 double c_ = 0;
  36                 RRTNode *p_ = nullptr;
  37                 unsigned int t_ = 0;
  38         public:
  39                 // getters, setters
  40                 double c() const { return this->c_; }
  41                 void c(double c) { this->c_ = c; }
  42
  43                 RRTNode *p() const { return this->p_; }
  44                 void p(RRTNode *p) { this->p_ = p; }
  45
  46                 bool t(unsigned int flag) { return this->t_ & flag; }
  47                 void set_t(unsigned int flag) { this->t_ |= flag; }
  48                 void clear_t(unsigned int flag) { this->t_ &= ~flag; }
  49
  50                 RRTNode();
  51                 RRTNode(const BicycleCar &bc);
  52 };
  53
  54 /*! \brief Polygon obstacle basic class.
  55
  56 \param poly Border polygon of the obstacle.
  57 */
  58 class Obstacle {
  59         private:
  60                 std::vector<std::tuple<double, double>> poly_;
  61         public:
  62                 // getters, setters
  63                 std::vector<std::tuple<double, double>> &poly()
  64                 {
  65                         return this->poly_;
  66                 }
  67
  68                 Obstacle();
  69 };
  70
  71 /*! \brief RRT* algorithm basic class.
  72
  73 \param icnt RRT algorithm iterations counter.
  74 \param goals The vector of goal nodes.
  75 \param nodes The vector of all nodes in RRT data structure.
  76 \param samples The vector of all samples of RRT algorithm.
  77 */
  78 class RRTS {
  79         private:
  80                 unsigned int icnt_ = 0;
  81                 std::chrono::high_resolution_clock::time_point tstart_;
  82                 double scnt_ = 0;
  83                 bool gf_ = false;
  84
  85                 std::vector<RRTNode> goals_;
  86                 std::vector<RRTNode> nodes_;
  87                 std::vector<Obstacle> obstacles_;
  88                 std::vector<RRTNode> samples_;
  89                 std::vector<RRTNode> steered_;
  90
  91                 /*! \brief Update and return elapsed time.
  92                 */
  93                 double elapsed();
  94                 /*! \brief Return ``true`` if algorithm should stop.
  95
  96                 Update counters (iteration, seconds, ...) and return if
  97                 the current iteration should be the last one.
  98                 */
  99                 bool should_stop();
 100         protected:
 101                 /*! \brief Store RRT node to tree data structure.
 102                 */
 103                 virtual void store_node(RRTNode n);
 104
 105                 // RRT procedures
 106                 std::tuple<bool, unsigned int, unsigned int>
 107                 collide(std::vector<std::tuple<double, double>> &poly);
 108                 virtual std::tuple<bool, unsigned int, unsigned int>
 109                 collide_steered_from(RRTNode &f);
 110                 virtual std::tuple<bool, unsigned int, unsigned int>
 111                 collide_two_nodes(RRTNode &f, RRTNode &t);
 112                 void sample();
 113                         std::default_random_engine gen_;
 114                         std::normal_distribution<double> ndx_;
 115                         std::normal_distribution<double> ndy_;
 116                         std::normal_distribution<double> ndh_;
 117                 virtual RRTNode *nn(RRTNode &t);
 118                 virtual std::vector<RRTNode *> nv(RRTNode &t);
 119                 void steer(RRTNode &f, RRTNode &t);
 120                 /*! \brief Join steered nodes to RRT data structure
 121
 122                 \param f RRT node to join steered nodes to.
 123                 */
 124                 void join_steered(RRTNode *f);
 125                 bool goal_found(RRTNode &f);
 126                 // RRT* procedures
 127                 bool connect();
 128                 void rewire();
 129         public:
 130                 /*! \brief Initialize RRT algorithm if needed.
 131                 */
 132                 virtual void init();
 133                 /*! \brief Deinitialize RRT algorithm if needed.
 134                 */
 135                 virtual void deinit();
 136                 /*! \brief Return path found by RRT*.
 137                 */
 138                 virtual std::vector<RRTNode *> path();
 139                 /*! \brief Run next RRT* iteration.
 140                 */
 141                 bool next();
 142                 /*! \brief Set sampling info.
 143
 144                 There is normal distribution sampling for `x`, `y`, and
 145                 `h` parameters of RRT node.
 146
 147                 \param mx Mean x value.
 148                 \param dx Standard deviation of x.
 149                 \param my Mean y value.
 150                 \param dy Standard deviation of y.
 151                 \param mh Mean h value.
 152                 \param dh Standard deviation of h.
 153                 */
 154                 void set_sample(
 155                         double mx, double dx,
 156                         double my, double dy,
 157                         double mh, double dh
 158                 );
 159
 160                 // RRT procedures
 161                 virtual double cost_build(RRTNode &f, RRTNode &t);
 162                 virtual double cost_search(RRTNode &f, RRTNode &t);
 163
 164                 // getters, setters
 165                 unsigned int icnt() const { return this->icnt_; }
 166                 double scnt() const { return this->scnt_; }
 167                 bool gf() const { return this->gf_; }
 168                 void gf(bool f) { this->gf_ = f; }
 169                 std::vector<RRTNode> &goals() { return this->goals_; }
 170                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 171                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 172                 std::vector<RRTNode> &samples() { return this->samples_; }
 173                 std::vector<RRTNode> &steered() { return this->steered_; }
 174
 175                 RRTS();
 176 };
 177
 178 /*! \brief Compute cumulative cost of RRT node.
 179
 180 \param t RRT node to compute cumulative cost to.
 181 */
 182 double cc(RRTNode &t);
 183
 184 #endif /* RRTS_H */