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                 RRTNode(const BicycleCar &bc);
  29 };
  30
  31 /*! \brief Polygon obstacle basic class.
  32
  33 \param poly Border polygon of the obstacle.
  34 */
  35 class Obstacle {
  36         private:
  37                 std::vector<std::tuple<double, double>> poly_;
  38         public:
  39                 // getters, setters
  40                 std::vector<std::tuple<double, double>> &poly()
  41                 {
  42                         return this->poly_;
  43                 }
  44
  45                 Obstacle();
  46 };
  47
  48 /*! \brief RRT* algorithm basic class.
  49
  50 \param icnt RRT algorithm iterations counter.
  51 \param goals The vector of goal nodes.
  52 \param nodes The vector of all nodes in RRT data structure.
  53 \param samples The vector of all samples of RRT algorithm.
  54 */
  55 class RRTS {
  56         private:
  57                 unsigned int icnt_ = 0;
  58
  59                 std::vector<RRTNode> goals_;
  60                 std::vector<RRTNode> nodes_;
  61                 std::vector<Obstacle> obstacles_;
  62                 std::vector<RRTNode> samples_;
  63                 std::vector<RRTNode> steered_;
  64
  65                 // RRT procedures
  66                 std::tuple<bool, unsigned int, unsigned int>
  67                 collide(std::vector<std::tuple<double, double>> &poly);
  68                 std::tuple<bool, unsigned int, unsigned int>
  69                 collide_steered_from(RRTNode &f);
  70                 std::tuple<bool, unsigned int, unsigned int>
  71                 collide_two_nodes(RRTNode &f, RRTNode &t);
  72                 double cost(RRTNode &f, RRTNode &t);
  73                 double cost_build(RRTNode &f, RRTNode &t);
  74                 double cost_search(RRTNode &f, RRTNode &t);
  75                 void sample();
  76                         std::default_random_engine gen_;
  77                         std::normal_distribution<double> ndx_;
  78                         std::normal_distribution<double> ndy_;
  79                         std::normal_distribution<double> ndh_;
  80                 RRTNode *nn(RRTNode &t);
  81                 std::vector<RRTNode *> nv(RRTNode &t);
  82                 void steer(RRTNode &f, RRTNode &t);
  83                 /*! \brief Join steered nodes to RRT data structure
  84
  85                 \param f RRT node to join steered nodes to.
  86                 */
  87                 void join_steered(RRTNode *f);
  88                 bool goal_found(RRTNode &f);
  89                 // RRT* procedures
  90                 bool connect();
  91                 void rewire();
  92         public:
  93                 /*! \brief Return path found by RRT*.
  94                 */
  95                 std::vector<RRTNode *> path();
  96                 /*! \brief Run next RRT* iteration.
  97                 */
  98                 bool next();
  99                 /*! \brief Set sampling info.
 100
 101                 There is normal distribution sampling for `x`, `y`, and
 102                 `h` parameters of RRT node.
 103
 104                 \param mx Mean x value.
 105                 \param dx Standard deviation of x.
 106                 \param my Mean y value.
 107                 \param dy Standard deviation of y.
 108                 \param mh Mean h value.
 109                 \param dh Standard deviation of h.
 110                 */
 111                 void set_sample(
 112                         double mx, double dx,
 113                         double my, double dy,
 114                         double mh, double dh
 115                 );
 116
 117                 // getters, setters
 118                 std::vector<RRTNode> &goals() { return this->goals_; }
 119                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 120                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 121                 std::vector<RRTNode> &samples() { return this->samples_; }
 122                 std::vector<RRTNode> &steered() { return this->steered_; }
 123
 124                 RRTS();
 125 };
 126
 127 /*! \brief Compute cumulative cost of RRT node.
 128
 129 \param t RRT node to compute cumulative cost to.
 130 */
 131 double cc(RRTNode &t);
 132
 133 #endif /* RRTS_H */