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                 /*! \brief Store RRT node to tree data structure.
  95                 */
  96                 void store_node(RRTNode n);
  97         protected:
  98                 // RRT procedures
  99                 std::tuple<bool, unsigned int, unsigned int>
 100                 collide(std::vector<std::tuple<double, double>> &poly);
 101                 virtual std::tuple<bool, unsigned int, unsigned int>
 102                 collide_steered_from(RRTNode &f);
 103                 virtual std::tuple<bool, unsigned int, unsigned int>
 104                 collide_two_nodes(RRTNode &f, RRTNode &t);
 105                 virtual double cost_build(RRTNode &f, RRTNode &t);
 106                 virtual double cost_search(RRTNode &f, RRTNode &t);
 107                 void sample();
 108                         std::default_random_engine gen_;
 109                         std::normal_distribution<double> ndx_;
 110                         std::normal_distribution<double> ndy_;
 111                         std::normal_distribution<double> ndh_;
 112                 RRTNode *nn(RRTNode &t);
 113                 std::vector<RRTNode *> nv(RRTNode &t);
 114                 void steer(RRTNode &f, RRTNode &t);
 115                 /*! \brief Join steered nodes to RRT data structure
 116
 117                 \param f RRT node to join steered nodes to.
 118                 */
 119                 void join_steered(RRTNode *f);
 120                 bool goal_found(RRTNode &f);
 121                 // RRT* procedures
 122                 bool connect();
 123                 void rewire();
 124         public:
 125                 /*! \brief Initialize RRT algorithm if needed.
 126                 */
 127                 virtual void init();
 128                 /*! \brief Deinitialize RRT algorithm if needed.
 129                 */
 130                 virtual void deinit();
 131                 /*! \brief Return path found by RRT*.
 132                 */
 133                 virtual std::vector<RRTNode *> path();
 134                 /*! \brief Run next RRT* iteration.
 135                 */
 136                 bool next();
 137                 /*! \brief Set sampling info.
 138
 139                 There is normal distribution sampling for `x`, `y`, and
 140                 `h` parameters of RRT node.
 141
 142                 \param mx Mean x value.
 143                 \param dx Standard deviation of x.
 144                 \param my Mean y value.
 145                 \param dy Standard deviation of y.
 146                 \param mh Mean h value.
 147                 \param dh Standard deviation of h.
 148                 */
 149                 void set_sample(
 150                         double mx, double dx,
 151                         double my, double dy,
 152                         double mh, double dh
 153                 );
 154
 155                 // getters, setters
 156                 unsigned int icnt() const { return this->icnt_; }
 157                 double scnt() const { return this->scnt_; }
 158                 bool gf() const { return this->gf_; }
 159                 void gf(bool f) { this->gf_ = f; }
 160                 std::vector<RRTNode> &goals() { return this->goals_; }
 161                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 162                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 163                 std::vector<RRTNode> &samples() { return this->samples_; }
 164                 std::vector<RRTNode> &steered() { return this->steered_; }
 165
 166                 RRTS();
 167 };
 168
 169 /*! \brief Compute cumulative cost of RRT node.
 170
 171 \param t RRT node to compute cumulative cost to.
 172 */
 173 double cc(RRTNode &t);
 174
 175 #endif /* RRTS_H */