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