api/rrts.h

   1 #ifndef RRTS_H
   2 #define RRTS_H
   3
   4 #include <functional>
   5 #include <random>
   6 #include <vector>
   7 #include "bcar.h"
   8
   9 /*! \brief RRT node basic class.
  10
  11 \param c Cumulative cost from RRT data structure root.
  12 \param p Pointer to parent RRT node.
  13 \param ch The vector of pointers to children RRT nodes.
  14 */
  15 class RRTNode : public BicycleCar {
  16         private:
  17                 double c_ = 0;
  18                 RRTNode *p_ = nullptr;
  19         public:
  20                 // getters, setters
  21                 double c() const { return this->c_; }
  22                 void c(double c) { this->c_ = c; }
  23
  24                 RRTNode *p() const { return this->p_; }
  25                 void p(RRTNode *p) { this->p_ = p; }
  26
  27                 RRTNode();
  28 };
  29
  30 /*! \brief Polygon obstacle basic class.
  31
  32 \param poly Border polygon of the obstacle.
  33 */
  34 class Obstacle {
  35         private:
  36                 std::vector<std::tuple<double, double>> poly_;
  37         public:
  38                 // getters, setters
  39                 std::vector<std::tuple<double, double>> &poly()
  40                 {
  41                         return this->poly_;
  42                 }
  43
  44                 Obstacle();
  45 };
  46
  47 /*! \brief RRT* algorithm basic class.
  48
  49 \param icnt RRT algorithm iterations counter.
  50 \param goals The vector of goal nodes.
  51 \param nodes The vector of all nodes in RRT data structure.
  52 \param samples The vector of all samples of RRT algorithm.
  53 */
  54 class RRTS {
  55         private:
  56                 unsigned int icnt_ = 0;
  57
  58                 std::vector<RRTNode> goals_;
  59                 std::vector<RRTNode> nodes_;
  60                 std::vector<Obstacle> obstacles_;
  61                 std::vector<RRTNode> samples_;
  62                 std::vector<RRTNode> steered_;
  63
  64                 // RRT procedures
  65                 bool collide(std::vector<std::tuple<double, double>> &poly);
  66                 bool collide_steered_from(RRTNode &f);
  67                 bool collide_two_nodes(RRTNode &f, RRTNode &t);
  68                 double cost(RRTNode &f, RRTNode &t);
  69                 void sample();
  70                         std::default_random_engine gen_;
  71                         std::normal_distribution<double> ndx_;
  72                         std::normal_distribution<double> ndy_;
  73                         std::normal_distribution<double> ndh_;
  74                 RRTNode *nn(RRTNode &t);
  75                 std::vector<RRTNode *> nv(RRTNode &t);
  76                 void steer(RRTNode &f, RRTNode &t);
  77                 // RRT* procedures
  78                 bool connect();
  79                 void rewire();
  80         public:
  81                 /*! \brief Return path found by RRT*.
  82                 */
  83                 std::vector<RRTNode *> path();
  84                 /*! \brief Run next RRT* iteration.
  85                 */
  86                 bool next();
  87                 /*! \brief Set sampling info.
  88
  89                 There is normal distribution sampling for `x`, `y`, and
  90                 `h` parameters of RRT node.
  91
  92                 \param mx Mean x value.
  93                 \param dx Standard deviation of x.
  94                 \param my Mean y value.
  95                 \param dy Standard deviation of y.
  96                 \param mh Mean h value.
  97                 \param dh Standard deviation of h.
  98                 */
  99                 void set_sample(
 100                         double mx, double dx,
 101                         double my, double dy,
 102                         double mh, double dh
 103                 );
 104
 105                 // getters, setters
 106                 std::vector<RRTNode> &goals() { return this->goals_; }
 107                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 108                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 109                 std::vector<RRTNode> &samples() { return this->samples_; }
 110                 std::vector<RRTNode> &steered() { return this->steered_; }
 111
 112                 RRTS();
 113 };
 114
 115 /*! \brief Compute cumulative cost of RRT node.
 116
 117 \param t RRT node to compute cumulative cost to.
 118 */
 119 double cc(RRTNode &t);
 120
 121 #endif /* RRTS_H */