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 t Type of the RRT node (RRTNodeType).
  33 // -- from BicycleCar
  34 \param x Horizontal coordinate of rear axle center.
  35 \param y Vertical coordinate of rear axle center.
  36 \param h Heading of the car in the interval [-pi,+pi] radians.
  37 \param sp Speed of the car.
  38 \param st Steering of the car.
  39 */
  40 class RRTNode {
  41         private:
  42                 double c_ = 0;
  43                 RRTNode *p_ = nullptr;
  44                 unsigned int t_ = 0;
  45                 // -- from BicycleCar
  46                 // coordinates
  47                 double x_ = 0;
  48                 double y_ = 0;
  49                 double h_ = 0;
  50                 // moving
  51                 double sp_ = 0;
  52                 double st_ = 0;
  53         public:
  54                 // getters, setters
  55                 double c() const { return this->c_; }
  56                 void c(double c) { this->c_ = c; }
  57
  58                 RRTNode *p() const { return this->p_; }
  59                 void p(RRTNode *p) { this->p_ = p; }
  60
  61                 bool t(unsigned int flag) { return this->t_ & flag; }
  62                 void set_t(unsigned int flag) { this->t_ |= flag; }
  63                 void clear_t(unsigned int flag) { this->t_ &= ~flag; }
  64
  65                 // -- from BicycleCar
  66                 // getters, setters
  67                 double x() const { return this->x_; }
  68                 void x(double x) { this->x_ = x; }
  69
  70                 double y() const { return this->y_; }
  71                 void y(double y) { this->y_ = y; }
  72
  73                 double h() const { return this->h_; }
  74                 void h(double h)
  75                 {
  76                         while (h < -M_PI)
  77                                 h += 2 * M_PI;
  78                         while (h > +M_PI)
  79                                 h -= 2 * M_PI;
  80                         this->h_ = h;
  81                 }
  82
  83                 double sp() const { return this->sp_; }
  84                 void sp(double sp) { this->sp_ = sp; }
  85
  86                 double st() const { return this->st_; }
  87                 void st(double st) { this->st_ = st; }
  88
  89                 RRTNode();
  90                 RRTNode(const BicycleCar &bc);
  91                 bool operator==(const RRTNode& n);
  92 };
  93
  94 /*! \brief Polygon obstacle basic class.
  95
  96 \param poly Border polygon of the obstacle.
  97 */
  98 class Obstacle {
  99         private:
 100                 std::vector<std::tuple<double, double>> poly_;
 101         public:
 102                 // getters, setters
 103                 std::vector<std::tuple<double, double>> &poly()
 104                 {
 105                         return this->poly_;
 106                 }
 107
 108                 Obstacle();
 109 };
 110
 111 /*! \brief RRT* algorithm basic class.
 112
 113 \param icnt RRT algorithm iterations counter.
 114 \param goals The vector of goal nodes.
 115 \param nodes The vector of all nodes in RRT data structure.
 116 \param samples The vector of all samples of RRT algorithm.
 117 \param sample_dist_type Random distribution type for sampling function (0 -
 118 normal, 1 - uniform, 2 - uniform circle)
 119 */
 120 class RRTS {
 121         private:
 122                 unsigned int icnt_ = 0;
 123                 std::chrono::high_resolution_clock::time_point tstart_;
 124                 double scnt_ = 0;
 125                 double pcnt_ = 0;
 126                 bool gf_ = false;
 127                 int sample_dist_type_ = 0;
 128
 129                 std::vector<RRTNode> goals_;
 130                 std::vector<RRTNode> nodes_;
 131                 std::vector<Obstacle> obstacles_;
 132                 std::vector<RRTNode> samples_;
 133                 std::vector<RRTNode> steered_;
 134                 double log_path_time_ = 0.1;
 135                 std::vector<double> log_path_cost_;
 136
 137                 /*! \brief Update and return elapsed time.
 138                 */
 139                 double elapsed();
 140                 /*! \brief Log current path cost.
 141                 */
 142                 void log_path_cost();
 143                 /*! \brief Set normal distribution for sampling.
 144                 */
 145                 void set_sample_normal(
 146                         double x1, double x2,
 147                         double y1, double y2,
 148                         double h1, double h2
 149                 );
 150                 /*! \brief Set uniform distribution for sampling.
 151                 */
 152                 void set_sample_uniform(
 153                         double x1, double x2,
 154                         double y1, double y2,
 155                         double h1, double h2
 156                 );
 157                 /*! \brief Set uniform circle distribution for sampling.
 158                 */
 159                 void set_sample_uniform_circle();
 160         protected:
 161                 double path_cost_before_opt_ = 9999;
 162
 163                 BicycleCar bc;
 164                 /*! \brief Store RRT node to tree data structure.
 165                 */
 166                 virtual void store_node(RRTNode n);
 167
 168                 // RRT procedures
 169                 std::tuple<bool, unsigned int, unsigned int>
 170                 collide(std::vector<std::tuple<double, double>> &poly);
 171                 virtual std::tuple<bool, unsigned int, unsigned int>
 172                 collide_steered_from(RRTNode &f);
 173                 virtual std::tuple<bool, unsigned int, unsigned int>
 174                 collide_two_nodes(RRTNode &f, RRTNode &t);
 175                 void sample();
 176                         std::default_random_engine gen_;
 177                         std::normal_distribution<double> ndx_;
 178                         std::normal_distribution<double> ndy_;
 179                         std::normal_distribution<double> ndh_;
 180                         std::uniform_real_distribution<double> udx_;
 181                         std::uniform_real_distribution<double> udy_;
 182                         std::uniform_real_distribution<double> udh_;
 183                 virtual RRTNode *nn(RRTNode &t);
 184                 virtual std::vector<RRTNode *> nv(RRTNode &t);
 185                 void steer(RRTNode &f, RRTNode &t);
 186                 /*! \brief Join steered nodes to RRT data structure
 187
 188                 \param f RRT node to join steered nodes to.
 189                 */
 190                 void join_steered(RRTNode *f);
 191                 virtual bool goal_found(RRTNode &f);
 192                 // RRT* procedures
 193                 bool connect();
 194                 void rewire();
 195         public:
 196                 /*! \brief Initialize RRT algorithm if needed.
 197                 */
 198                 virtual void init();
 199                 /*! \brief Deinitialize RRT algorithm if needed.
 200                 */
 201                 virtual void deinit();
 202                 /*! \brief Return path found by RRT*.
 203                 */
 204                 virtual std::vector<RRTNode *> path();
 205                 /*! \brief Return ``true`` if algorithm should stop.
 206
 207                 Update counters (iteration, seconds, ...) and return if
 208                 the current iteration should be the last one.
 209                 */
 210                 bool should_stop();
 211                 /*! \brief Return ``true`` if the algorithm should finish.
 212
 213                 Finish means that the algorithm will not be resumed.
 214                 */
 215                 bool should_finish();
 216                 /*! \brief Return ``true`` if the algorithm shoud break.
 217
 218                 Break means that the algorithm can be resumed.
 219                 */
 220                 bool should_break();
 221                 /*! \brief Return ``true`` if algorithm should continue.
 222
 223                 `pcnt_` is set to `scnt_`, so the difference is 0 and it can
 224                 start from scratch. After the `should_continue` is called,
 225                 there must be `while (rrts.next()) {}` loop.
 226                 */
 227                 bool should_continue();
 228                 /*! \brief Run next RRT* iteration.
 229                 */
 230                 bool next();
 231                 /*! \brief Set sampling info.
 232
 233                 Based on `sample_dist_type`, set proper distribution
 234                 parameters. The distribution parameters are relative to `front`
 235                 node in `nodes` (init).
 236
 237                 For normal sampling:
 238                 \param x1 Mean x value.
 239                 \param x2 Standard deviation of x.
 240                 \param y1 Mean y value.
 241                 \param y2 Standard deviation of y.
 242                 \param h1 Mean h value.
 243                 \param h2 Standard deviation of h.
 244
 245                 For uniform sampling:
 246                 \param x1 Minimum x value.
 247                 \param x2 Maximum x value.
 248                 \param y1 Minimum y value.
 249                 \param y2 Maximum y value.
 250                 \param h1 Minimum h value.
 251                 \param h2 Maximum h value.
 252
 253                 For uniform circle sampling:
 254                 \param x1 Ignored.
 255                 \param x2 Ignored.
 256                 \param y1 Ignored.
 257                 \param y2 Ignored.
 258                 \param h1 Ignored.
 259                 \param h2 Ignored.
 260                 */
 261                 void set_sample(
 262                         double x1, double x2,
 263                         double y1, double y2,
 264                         double h1, double h2
 265                 );
 266                 /*! \brief Generate JSON output.
 267                 */
 268                 Json::Value json();
 269                 /*! \brief Load JSON input.
 270                 */
 271                 void json(Json::Value jvi);
 272
 273                 // RRT procedures
 274                 virtual double cost_build(RRTNode &f, RRTNode &t);
 275                 virtual double cost_search(RRTNode &f, RRTNode &t);
 276
 277                 // getters, setters
 278                 unsigned int icnt() const { return this->icnt_; }
 279                 double scnt() const { return this->scnt_; }
 280                 bool gf() const { return this->gf_; }
 281                 void gf(bool f) { this->gf_ = f; }
 282                 int sample_dist_type() const { return this->sample_dist_type_;}
 283                 void sample_dist_type(int t) { this->sample_dist_type_ = t; }
 284                 std::vector<RRTNode> &goals() { return this->goals_; }
 285                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 286                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 287                 std::vector<RRTNode> &samples() { return this->samples_; }
 288                 std::vector<RRTNode> &steered() { return this->steered_; }
 289
 290                 RRTS();
 291 };
 292
 293 /*! \brief Compute cumulative cost of RRT node.
 294
 295 \param t RRT node to compute cumulative cost to.
 296 */
 297 double cc(RRTNode &t);
 298
 299 #endif /* RRTS_H */