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
  62                 std::vector<RRTNode> goals_;
  63                 std::vector<RRTNode> nodes_;
  64                 std::vector<Obstacle> obstacles_;
  65                 std::vector<RRTNode> samples_;
  66                 std::vector<RRTNode> steered_;
  67
  68                 /*! \brief Update and return elapsed time.
  69                 */
  70                 double elapsed();
  71                 /*! \brief Return ``true`` if algorithm should stop.
  72
  73                 Update counters (iteration, seconds, ...) and return if
  74                 the current iteration should be the last one.
  75                 */
  76                 bool should_stop();
  77
  78                 // RRT procedures
  79                 std::tuple<bool, unsigned int, unsigned int>
  80                 collide(std::vector<std::tuple<double, double>> &poly);
  81                 virtual std::tuple<bool, unsigned int, unsigned int>
  82                 collide_steered_from(RRTNode &f);
  83                 virtual std::tuple<bool, unsigned int, unsigned int>
  84                 collide_two_nodes(RRTNode &f, RRTNode &t);
  85                 virtual double cost_build(RRTNode &f, RRTNode &t);
  86                 virtual double cost_search(RRTNode &f, RRTNode &t);
  87                 void sample();
  88                         std::default_random_engine gen_;
  89                         std::normal_distribution<double> ndx_;
  90                         std::normal_distribution<double> ndy_;
  91                         std::normal_distribution<double> ndh_;
  92                 RRTNode *nn(RRTNode &t);
  93                 std::vector<RRTNode *> nv(RRTNode &t);
  94                 void steer(RRTNode &f, RRTNode &t);
  95                 /*! \brief Join steered nodes to RRT data structure
  96
  97                 \param f RRT node to join steered nodes to.
  98                 */
  99                 void join_steered(RRTNode *f);
 100                 bool goal_found(RRTNode &f);
 101                 // RRT* procedures
 102                 bool connect();
 103                 void rewire();
 104         public:
 105                 /*! \brief Return path found by RRT*.
 106                 */
 107                 std::vector<RRTNode *> path();
 108                 /*! \brief Run next RRT* iteration.
 109                 */
 110                 bool next();
 111                 /*! \brief Set sampling info.
 112
 113                 There is normal distribution sampling for `x`, `y`, and
 114                 `h` parameters of RRT node.
 115
 116                 \param mx Mean x value.
 117                 \param dx Standard deviation of x.
 118                 \param my Mean y value.
 119                 \param dy Standard deviation of y.
 120                 \param mh Mean h value.
 121                 \param dh Standard deviation of h.
 122                 */
 123                 void set_sample(
 124                         double mx, double dx,
 125                         double my, double dy,
 126                         double mh, double dh
 127                 );
 128
 129                 // getters, setters
 130                 unsigned int icnt() const { return this->icnt_; }
 131                 double scnt() const { return this->scnt_; }
 132                 std::vector<RRTNode> &goals() { return this->goals_; }
 133                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 134                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 135                 std::vector<RRTNode> &samples() { return this->samples_; }
 136                 std::vector<RRTNode> &steered() { return this->steered_; }
 137
 138                 RRTS();
 139 };
 140
 141 /*! \brief Compute cumulative cost of RRT node.
 142
 143 \param t RRT node to compute cumulative cost to.
 144 */
 145 double cc(RRTNode &t);
 146
 147 #endif /* RRTS_H */