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                 double pcnt_ = 0;
  85                 bool gf_ = false;
  86
  87                 std::vector<RRTNode> goals_;
  88                 std::vector<RRTNode> nodes_;
  89                 std::vector<Obstacle> obstacles_;
  90                 std::vector<RRTNode> samples_;
  91                 std::vector<RRTNode> steered_;
  92
  93                 /*! \brief Update and return elapsed time.
  94                 */
  95                 double elapsed();
  96         protected:
  97                 /*! \brief Store RRT node to tree data structure.
  98                 */
  99                 virtual void store_node(RRTNode n);
 100
 101                 // RRT procedures
 102                 std::tuple<bool, unsigned int, unsigned int>
 103                 collide(std::vector<std::tuple<double, double>> &poly);
 104                 virtual std::tuple<bool, unsigned int, unsigned int>
 105                 collide_steered_from(RRTNode &f);
 106                 virtual std::tuple<bool, unsigned int, unsigned int>
 107                 collide_two_nodes(RRTNode &f, RRTNode &t);
 108                 void sample();
 109                         std::default_random_engine gen_;
 110                         std::normal_distribution<double> ndx_;
 111                         std::normal_distribution<double> ndy_;
 112                         std::normal_distribution<double> ndh_;
 113                 virtual RRTNode *nn(RRTNode &t);
 114                 virtual std::vector<RRTNode *> nv(RRTNode &t);
 115                 void steer(RRTNode &f, RRTNode &t);
 116                 /*! \brief Join steered nodes to RRT data structure
 117
 118                 \param f RRT node to join steered nodes to.
 119                 */
 120                 void join_steered(RRTNode *f);
 121                 bool goal_found(RRTNode &f);
 122                 // RRT* procedures
 123                 bool connect();
 124                 void rewire();
 125         public:
 126                 /*! \brief Initialize RRT algorithm if needed.
 127                 */
 128                 virtual void init();
 129                 /*! \brief Deinitialize RRT algorithm if needed.
 130                 */
 131                 virtual void deinit();
 132                 /*! \brief Return path found by RRT*.
 133                 */
 134                 virtual std::vector<RRTNode *> path();
 135                 /*! \brief Return ``true`` if algorithm should stop.
 136
 137                 Update counters (iteration, seconds, ...) and return if
 138                 the current iteration should be the last one.
 139                 */
 140                 bool should_stop();
 141                 /*! \brief Return ``true`` if the algorithm should finish.
 142
 143                 Finish means that the algorithm will not be resumed.
 144                 */
 145                 bool should_finish();
 146                 /*! \brief Return ``true`` if the algorithm shoud break.
 147
 148                 Break means that the algorithm can be resumed.
 149                 */
 150                 bool should_break();
 151                 /*! \brief Return ``true`` if algorithm should continue.
 152
 153                 `pcnt_` is set to `scnt_`, so the difference is 0 and it can
 154                 start from scratch. After the `should_continue` is called,
 155                 there must be `while (rrts.next()) {}` loop.
 156                 */
 157                 bool should_continue();
 158                 /*! \brief Run next RRT* iteration.
 159                 */
 160                 bool next();
 161                 /*! \brief Set sampling info.
 162
 163                 There is normal distribution sampling for `x`, `y`, and
 164                 `h` parameters of RRT node.
 165
 166                 \param mx Mean x value.
 167                 \param dx Standard deviation of x.
 168                 \param my Mean y value.
 169                 \param dy Standard deviation of y.
 170                 \param mh Mean h value.
 171                 \param dh Standard deviation of h.
 172                 */
 173                 void set_sample(
 174                         double mx, double dx,
 175                         double my, double dy,
 176                         double mh, double dh
 177                 );
 178                 /*! \brief Generate JSON output.
 179                 */
 180                 Json::Value json();
 181                 /*! \brief Load JSON input.
 182                 */
 183                 void json(Json::Value jvi);
 184
 185                 // RRT procedures
 186                 virtual double cost_build(RRTNode &f, RRTNode &t);
 187                 virtual double cost_search(RRTNode &f, RRTNode &t);
 188
 189                 // getters, setters
 190                 unsigned int icnt() const { return this->icnt_; }
 191                 double scnt() const { return this->scnt_; }
 192                 bool gf() const { return this->gf_; }
 193                 void gf(bool f) { this->gf_ = f; }
 194                 std::vector<RRTNode> &goals() { return this->goals_; }
 195                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 196                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 197                 std::vector<RRTNode> &samples() { return this->samples_; }
 198                 std::vector<RRTNode> &steered() { return this->steered_; }
 199
 200                 RRTS();
 201 };
 202
 203 /*! \brief Compute cumulative cost of RRT node.
 204
 205 \param t RRT node to compute cumulative cost to.
 206 */
 207 double cc(RRTNode &t);
 208
 209 #endif /* RRTS_H */