api/rrts.h

   1 #ifndef RRTS_H
   2 #define RRTS_H
   3
   4 #include <chrono>
   5 #include <functional>
   6 #include <jsoncpp/json/json.h>
   7 #include <random>
   8 #include <vector>
   9 #include "bcar.h"
  10
  11 #define ETA 1.0 // for steer, nv
  12 #define GAMMA(cV) ({ \
  13         __typeof__ (cV) _cV = (cV); \
  14         pow(log(_cV) / _cV, 1.0 / 3.0); \
  15 })
  16
  17 /*! \brief Possible type of RRT node.
  18
  19 \param cusp The node that is cusp (change in direction).
  20 \param connected The node that branches generated steered path.
  21 */
  22 class RRTNodeType {
  23         public:
  24                 static const unsigned int cusp = 1 << 0;
  25                 static const unsigned int connected = 1 << 1;
  26 };
  27
  28 /*! \brief RRT node basic class.
  29
  30 \param c Cumulative cost from RRT data structure root.
  31 \param p Pointer to parent RRT node.
  32 \param ch The vector of pointers to children RRT nodes.
  33 */
  34 class RRTNode : public BicycleCar {
  35         private:
  36                 double c_ = 0;
  37                 RRTNode *p_ = nullptr;
  38                 unsigned int t_ = 0;
  39         public:
  40                 // getters, setters
  41                 double c() const { return this->c_; }
  42                 void c(double c) { this->c_ = c; }
  43
  44                 RRTNode *p() const { return this->p_; }
  45                 void p(RRTNode *p) { this->p_ = p; }
  46
  47                 bool t(unsigned int flag) { return this->t_ & flag; }
  48                 void set_t(unsigned int flag) { this->t_ |= flag; }
  49                 void clear_t(unsigned int flag) { this->t_ &= ~flag; }
  50
  51                 RRTNode();
  52                 RRTNode(const BicycleCar &bc);
  53 };
  54
  55 /*! \brief Polygon obstacle basic class.
  56
  57 \param poly Border polygon of the obstacle.
  58 */
  59 class Obstacle {
  60         private:
  61                 std::vector<std::tuple<double, double>> poly_;
  62         public:
  63                 // getters, setters
  64                 std::vector<std::tuple<double, double>> &poly()
  65                 {
  66                         return this->poly_;
  67                 }
  68
  69                 Obstacle();
  70 };
  71
  72 /*! \brief RRT* algorithm basic class.
  73
  74 \param icnt RRT algorithm iterations counter.
  75 \param goals The vector of goal nodes.
  76 \param nodes The vector of all nodes in RRT data structure.
  77 \param samples The vector of all samples of RRT algorithm.
  78 */
  79 class RRTS {
  80         private:
  81                 unsigned int icnt_ = 0;
  82                 std::chrono::high_resolution_clock::time_point tstart_;
  83                 double scnt_ = 0;
  84                 bool gf_ = false;
  85
  86                 std::vector<RRTNode> goals_;
  87                 std::vector<RRTNode> nodes_;
  88                 std::vector<Obstacle> obstacles_;
  89                 std::vector<RRTNode> samples_;
  90                 std::vector<RRTNode> steered_;
  91
  92                 /*! \brief Update and return elapsed time.
  93                 */
  94                 double elapsed();
  95                 /*! \brief Return ``true`` if algorithm should stop.
  96
  97                 Update counters (iteration, seconds, ...) and return if
  98                 the current iteration should be the last one.
  99                 */
 100                 bool should_stop();
 101         protected:
 102                 /*! \brief Store RRT node to tree data structure.
 103                 */
 104                 virtual void store_node(RRTNode n);
 105
 106                 // RRT procedures
 107                 std::tuple<bool, unsigned int, unsigned int>
 108                 collide(std::vector<std::tuple<double, double>> &poly);
 109                 virtual std::tuple<bool, unsigned int, unsigned int>
 110                 collide_steered_from(RRTNode &f);
 111                 virtual std::tuple<bool, unsigned int, unsigned int>
 112                 collide_two_nodes(RRTNode &f, RRTNode &t);
 113                 void sample();
 114                         std::default_random_engine gen_;
 115                         std::normal_distribution<double> ndx_;
 116                         std::normal_distribution<double> ndy_;
 117                         std::normal_distribution<double> ndh_;
 118                 virtual RRTNode *nn(RRTNode &t);
 119                 virtual std::vector<RRTNode *> nv(RRTNode &t);
 120                 void steer(RRTNode &f, RRTNode &t);
 121                 /*! \brief Join steered nodes to RRT data structure
 122
 123                 \param f RRT node to join steered nodes to.
 124                 */
 125                 void join_steered(RRTNode *f);
 126                 bool goal_found(RRTNode &f);
 127                 // RRT* procedures
 128                 bool connect();
 129                 void rewire();
 130         public:
 131                 /*! \brief Initialize RRT algorithm if needed.
 132                 */
 133                 virtual void init();
 134                 /*! \brief Deinitialize RRT algorithm if needed.
 135                 */
 136                 virtual void deinit();
 137                 /*! \brief Return path found by RRT*.
 138                 */
 139                 virtual std::vector<RRTNode *> path();
 140                 /*! \brief Run next RRT* iteration.
 141                 */
 142                 bool next();
 143                 /*! \brief Set sampling info.
 144
 145                 There is normal distribution sampling for `x`, `y`, and
 146                 `h` parameters of RRT node.
 147
 148                 \param mx Mean x value.
 149                 \param dx Standard deviation of x.
 150                 \param my Mean y value.
 151                 \param dy Standard deviation of y.
 152                 \param mh Mean h value.
 153                 \param dh Standard deviation of h.
 154                 */
 155                 void set_sample(
 156                         double mx, double dx,
 157                         double my, double dy,
 158                         double mh, double dh
 159                 );
 160                 /*! \brief Generate JSON output.
 161                 */
 162                 Json::Value json();
 163
 164                 // RRT procedures
 165                 virtual double cost_build(RRTNode &f, RRTNode &t);
 166                 virtual double cost_search(RRTNode &f, RRTNode &t);
 167
 168                 // getters, setters
 169                 unsigned int icnt() const { return this->icnt_; }
 170                 double scnt() const { return this->scnt_; }
 171                 bool gf() const { return this->gf_; }
 172                 void gf(bool f) { this->gf_ = f; }
 173                 std::vector<RRTNode> &goals() { return this->goals_; }
 174                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 175                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 176                 std::vector<RRTNode> &samples() { return this->samples_; }
 177                 std::vector<RRTNode> &steered() { return this->steered_; }
 178
 179                 RRTS();
 180 };
 181
 182 /*! \brief Compute cumulative cost of RRT node.
 183
 184 \param t RRT node to compute cumulative cost to.
 185 */
 186 double cc(RRTNode &t);
 187
 188 #endif /* RRTS_H */