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