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