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