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                 /*! \brief Set normal distribution for sampling.
 100                 */
 101                 void set_sample_normal(
 102                         double x1, double x2,
 103                         double y1, double y2,
 104                         double h1, double h2
 105                 );
 106                 /*! \brief Set uniform distribution for sampling.
 107                 */
 108                 void set_sample_uniform(
 109                         double x1, double x2,
 110                         double y1, double y2,
 111                         double h1, double h2
 112                 );
 113         protected:
 114                 /*! \brief Store RRT node to tree data structure.
 115                 */
 116                 virtual void store_node(RRTNode n);
 117
 118                 // RRT procedures
 119                 std::tuple<bool, unsigned int, unsigned int>
 120                 collide(std::vector<std::tuple<double, double>> &poly);
 121                 virtual std::tuple<bool, unsigned int, unsigned int>
 122                 collide_steered_from(RRTNode &f);
 123                 virtual std::tuple<bool, unsigned int, unsigned int>
 124                 collide_two_nodes(RRTNode &f, RRTNode &t);
 125                 void sample();
 126                         std::default_random_engine gen_;
 127                         std::normal_distribution<double> ndx_;
 128                         std::normal_distribution<double> ndy_;
 129                         std::normal_distribution<double> ndh_;
 130                         std::uniform_real_distribution<double> udx_;
 131                         std::uniform_real_distribution<double> udy_;
 132                         std::uniform_real_distribution<double> udh_;
 133                 virtual RRTNode *nn(RRTNode &t);
 134                 virtual std::vector<RRTNode *> nv(RRTNode &t);
 135                 void steer(RRTNode &f, RRTNode &t);
 136                 /*! \brief Join steered nodes to RRT data structure
 137
 138                 \param f RRT node to join steered nodes to.
 139                 */
 140                 void join_steered(RRTNode *f);
 141                 bool goal_found(RRTNode &f);
 142                 // RRT* procedures
 143                 bool connect();
 144                 void rewire();
 145         public:
 146                 /*! \brief Initialize RRT algorithm if needed.
 147                 */
 148                 virtual void init();
 149                 /*! \brief Deinitialize RRT algorithm if needed.
 150                 */
 151                 virtual void deinit();
 152                 /*! \brief Return path found by RRT*.
 153                 */
 154                 virtual std::vector<RRTNode *> path();
 155                 /*! \brief Return ``true`` if algorithm should stop.
 156
 157                 Update counters (iteration, seconds, ...) and return if
 158                 the current iteration should be the last one.
 159                 */
 160                 bool should_stop();
 161                 /*! \brief Return ``true`` if the algorithm should finish.
 162
 163                 Finish means that the algorithm will not be resumed.
 164                 */
 165                 bool should_finish();
 166                 /*! \brief Return ``true`` if the algorithm shoud break.
 167
 168                 Break means that the algorithm can be resumed.
 169                 */
 170                 bool should_break();
 171                 /*! \brief Return ``true`` if algorithm should continue.
 172
 173                 `pcnt_` is set to `scnt_`, so the difference is 0 and it can
 174                 start from scratch. After the `should_continue` is called,
 175                 there must be `while (rrts.next()) {}` loop.
 176                 */
 177                 bool should_continue();
 178                 /*! \brief Run next RRT* iteration.
 179                 */
 180                 bool next();
 181                 /*! \brief Set sampling info.
 182
 183                 Based on `sample_dist_type`, set proper distribution
 184                 parameters. The distribution parameters are relative to `front`
 185                 node in `nodes` (init).
 186
 187                 For normal sampling:
 188                 \param x1 Mean x value.
 189                 \param x2 Standard deviation of x.
 190                 \param y1 Mean y value.
 191                 \param y2 Standard deviation of y.
 192                 \param h1 Mean h value.
 193                 \param h2 Standard deviation of h.
 194
 195                 For uniform sampling:
 196                 \param x1 Minimum x value.
 197                 \param x2 Maximum x value.
 198                 \param y1 Minimum y value.
 199                 \param y2 Maximum y value.
 200                 \param h1 Minimum h value.
 201                 \param h2 Maximum h value.
 202                 */
 203                 void set_sample(
 204                         double x1, double x2,
 205                         double y1, double y2,
 206                         double h1, double h2
 207                 );
 208                 /*! \brief Generate JSON output.
 209                 */
 210                 Json::Value json();
 211                 /*! \brief Load JSON input.
 212                 */
 213                 void json(Json::Value jvi);
 214
 215                 // RRT procedures
 216                 virtual double cost_build(RRTNode &f, RRTNode &t);
 217                 virtual double cost_search(RRTNode &f, RRTNode &t);
 218
 219                 // getters, setters
 220                 unsigned int icnt() const { return this->icnt_; }
 221                 double scnt() const { return this->scnt_; }
 222                 bool gf() const { return this->gf_; }
 223                 void gf(bool f) { this->gf_ = f; }
 224                 int sample_dist_type() const { return this->sample_dist_type_;}
 225                 void sample_dist_type(int t) { this->sample_dist_type_ = t; }
 226                 std::vector<RRTNode> &goals() { return this->goals_; }
 227                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 228                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 229                 std::vector<RRTNode> &samples() { return this->samples_; }
 230                 std::vector<RRTNode> &steered() { return this->steered_; }
 231
 232                 RRTS();
 233 };
 234
 235 /*! \brief Compute cumulative cost of RRT node.
 236
 237 \param t RRT node to compute cumulative cost to.
 238 */
 239 double cc(RRTNode &t);
 240
 241 #endif /* RRTS_H */