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