incl/rrts.hh

   1 #ifndef RRTS_H
   2 #define RRTS_H
   3
   4 #include <chrono>
   5 #include <functional>
   6 #include <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                 std::vector<RRTNode *> path_;
 151                 double log_path_time_ = 0.1;
 152                 unsigned int log_path_iter_ = 20;
 153
 154                 /*! \brief Update and return elapsed time.
 155                 */
 156                 double elapsed();
 157                 /*! \brief Log current path cost.
 158                 */
 159                 void log_path_cost();
 160                 /*! \brief Set normal distribution for sampling.
 161                 */
 162                 void set_sample_normal(
 163                         double x1, double x2,
 164                         double y1, double y2,
 165                         double h1, double h2
 166                 );
 167                 /*! \brief Set uniform distribution for sampling.
 168                 */
 169                 void set_sample_uniform(
 170                         double x1, double x2,
 171                         double y1, double y2,
 172                         double h1, double h2
 173                 );
 174                 /*! \brief Set uniform circle distribution for sampling.
 175                 */
 176                 void set_sample_uniform_circle();
 177                 RRTNode* use_nn; // Used for RRTExt12.
 178                 std::vector<RRTNode> tmp_steered_;
 179                 bool finishit = false;
 180                 double path_cost_before_opt_ = 9999;
 181
 182                 BicycleCar bc;
 183                 /*! \brief Store RRT node to tree data structure.
 184                 */
 185                 virtual void store_node(RRTNode n);
 186
 187                 // RRT procedures
 188                 std::tuple<bool, unsigned int, unsigned int>
 189                 collide(std::vector<std::tuple<double, double>> &poly);
 190                 virtual std::tuple<bool, unsigned int, unsigned int>
 191                 collide_steered_from(RRTNode &f);
 192                 virtual std::tuple<bool, unsigned int, unsigned int>
 193                 collide_tmp_steered_from(RRTNode &f);
 194                 virtual std::tuple<bool, unsigned int, unsigned int>
 195                 collide_two_nodes(RRTNode &f, RRTNode &t);
 196                 void sample();
 197                         std::default_random_engine gen_;
 198                         std::normal_distribution<double> ndx_;
 199                         std::normal_distribution<double> ndy_;
 200                         std::normal_distribution<double> ndh_;
 201                         std::uniform_real_distribution<double> udx_;
 202                         std::uniform_real_distribution<double> udy_;
 203                         std::uniform_real_distribution<double> udh_;
 204                         std::uniform_int_distribution<unsigned int> udi1_;
 205                         std::uniform_int_distribution<unsigned int> udi2_;
 206                 virtual RRTNode *nn(RRTNode &t);
 207                 virtual std::vector<RRTNode *> nv(RRTNode &t);
 208                 void steer(RRTNode &f, RRTNode &t);
 209                 void tmp_steer(RRTNode &f, RRTNode &t);
 210                 virtual void steer1(RRTNode &f, RRTNode &t);
 211                 virtual void steer2(RRTNode &f, RRTNode &t);
 212                 /*! \brief Join steered nodes to RRT data structure
 213
 214                 \param f RRT node to join steered nodes to.
 215                 */
 216                 void join_steered(RRTNode *f);
 217                 void join_tmp_steered(RRTNode *f);
 218                 virtual bool goal_found(RRTNode &f);
 219                 // RRT* procedures
 220                 virtual bool connect();
 221                 void rewire();
 222         public:
 223                 /// ---
 224                 std::vector<double> log_opt_time_;
 225                 std::vector<double> log_path_cost_;
 226                 struct { double x=0; double y=0; double b=0; double e=0; } entry;
 227                 bool entry_set = false;
 228                 struct { double x=0; double y=0; double h=0; } entry1;
 229                 struct { double x=0; double y=0; double h=0; } entry2;
 230                 bool entries_set = false;
 231                 std::vector<RRTNode *> steered1_;
 232                 std::vector<RRTNode *> steered2_;
 233                 /// ---
 234                 /*! \brief Initialize RRT algorithm if needed.
 235                 */
 236                 virtual void init();
 237                 virtual void reset();
 238                 /*! \brief Deinitialize RRT algorithm if needed.
 239                 */
 240                 virtual void deinit();
 241                 /*! \brief Return path found by RRT*.
 242                 */
 243                 virtual std::vector<RRTNode *>& path()
 244                 {
 245                         return this->path_;
 246                 }
 247                 virtual void compute_path();
 248                 /*! \brief Return ``true`` if algorithm should stop.
 249
 250                 Update counters (iteration, seconds, ...) and return if
 251                 the current iteration should be the last one.
 252                 */
 253                 bool should_stop();
 254                 /*! \brief Return ``true`` if the algorithm should finish.
 255
 256                 Finish means that the algorithm will not be resumed.
 257                 */
 258                 bool should_finish();
 259                 /*! \brief Return ``true`` if the algorithm shoud break.
 260
 261                 Break means that the algorithm can be resumed.
 262                 */
 263                 bool should_break();
 264                 /*! \brief Return ``true`` if algorithm should continue.
 265
 266                 `pcnt_` is set to `scnt_`, so the difference is 0 and it can
 267                 start from scratch. After the `should_continue` is called,
 268                 there must be `while (rrts.next()) {}` loop.
 269                 */
 270                 bool should_continue();
 271                 /*! \brief Run next RRT* iteration.
 272                 */
 273                 virtual bool next();
 274                 /*! \brief Set sampling info.
 275
 276                 Based on `sample_dist_type`, set proper distribution
 277                 parameters. The distribution parameters are relative to `front`
 278                 node in `nodes` (init).
 279
 280                 For normal sampling:
 281                 \param x1 Mean x value.
 282                 \param x2 Standard deviation of x.
 283                 \param y1 Mean y value.
 284                 \param y2 Standard deviation of y.
 285                 \param h1 Mean h value.
 286                 \param h2 Standard deviation of h.
 287
 288                 For uniform sampling:
 289                 \param x1 Minimum x value.
 290                 \param x2 Maximum x value.
 291                 \param y1 Minimum y value.
 292                 \param y2 Maximum y value.
 293                 \param h1 Minimum h value.
 294                 \param h2 Maximum h value.
 295
 296                 For uniform circle sampling:
 297                 \param x1 Ignored.
 298                 \param x2 Ignored.
 299                 \param y1 Ignored.
 300                 \param y2 Ignored.
 301                 \param h1 Ignored.
 302                 \param h2 Ignored.
 303                 */
 304                 void set_sample(
 305                         double x1, double x2,
 306                         double y1, double y2,
 307                         double h1, double h2
 308                 );
 309                 /*! \brief Generate JSON output.
 310                 */
 311                 Json::Value json();
 312                 /*! \brief Load JSON input.
 313                 */
 314                 void json(Json::Value jvi);
 315
 316                 // RRT procedures
 317                 virtual double cost_build(RRTNode &f, RRTNode &t);
 318                 virtual double cost_search(RRTNode &f, RRTNode &t);
 319
 320                 // getters, setters
 321                 unsigned int icnt() const { return this->icnt_; }
 322                 void icnt(unsigned int i) { this->icnt_ = i; }
 323                 double scnt() const { return this->scnt_; }
 324                 bool gf() const { return this->gf_; }
 325                 void gf(bool f) { this->gf_ = f; }
 326                 int sample_dist_type() const { return this->sample_dist_type_;}
 327                 void sample_dist_type(int t) { this->sample_dist_type_ = t; }
 328                 std::vector<RRTNode> &goals() { return this->goals_; }
 329                 std::vector<RRTNode> &nodes() { return this->nodes_; }
 330                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
 331                 std::vector<RRTNode> &samples() { return this->samples_; }
 332                 std::vector<RRTNode> &steered() { return this->steered_; }
 333
 334                 RRTS();
 335 };
 336
 337 /*! \brief Compute cumulative cost of RRT node.
 338
 339 \param t RRT node to compute cumulative cost to.
 340 */
 341 double cc(RRTNode &t);
 342
 343 #endif /* RRTS_H */