src/rrts.cc

   1 #include <algorithm>
   2 #include "rrts.h"
   3
   4 #include "reeds_shepp.h"
   5 #include <iostream>
   6 template <typename T> int sgn(T val) {
   7         return (T(0) < val) - (val < T(0));
   8 }
   9
  10 RRTNode::RRTNode()
  11 {
  12 }
  13
  14 RRTNode::RRTNode(const BicycleCar &bc) : BicycleCar(bc)
  15 {
  16 }
  17
  18 bool RRTNode::operator==(const RRTNode& n)
  19 {
  20         if (this == &n)
  21                 return true;
  22         return false;
  23 }
  24
  25 Obstacle::Obstacle()
  26 {
  27 }
  28
  29 double RRTS::elapsed()
  30 {
  31         std::chrono::duration<double> dt;
  32         dt = std::chrono::duration_cast<std::chrono::duration<double>>(
  33                 std::chrono::high_resolution_clock::now()
  34                 - this->tstart_
  35         );
  36         this->scnt_ = dt.count();
  37         return this->scnt_;
  38 }
  39
  40 bool RRTS::should_stop()
  41 {
  42         // the following counters must be updated, do not comment
  43         this->icnt_++;
  44         this->elapsed();
  45         // current iteration stop conditions
  46         if (this->should_finish()) return true;
  47         if (this->should_break()) return true;
  48         // but continue by default
  49         return false;
  50 }
  51
  52 bool RRTS::should_finish()
  53 {
  54         // decide finish conditions (maybe comment some lines)
  55         //if (this->icnt_ > 999) return true;
  56         if (this->scnt_ > 50) return true;
  57         if (this->gf()) return true;
  58         // but continue by default
  59         return false;
  60 }
  61
  62 bool RRTS::should_break()
  63 {
  64         // decide break conditions (maybe comment some lines)
  65         //if (this->scnt_ - this->pcnt_ > 2) return true;
  66         // but continue by default
  67         return false;
  68 }
  69
  70 bool RRTS::should_continue()
  71 {
  72         // decide the stop conditions (maybe comment some lines)
  73         // it is exact opposite of `should_stop`
  74         //if (this->icnt_ > 999) return false;
  75         if (this->scnt_ > 10) return false;
  76         if (this->gf()) return false;
  77         // and reset pause counter if should continue
  78         this->pcnt_ = this->scnt_;
  79         return true;
  80 }
  81
  82 void RRTS::store_node(RRTNode n)
  83 {
  84         this->nodes().push_back(n);
  85 }
  86
  87 // RRT procedures
  88 std::tuple<bool, unsigned int, unsigned int>
  89 RRTS::collide(std::vector<std::tuple<double, double>> &poly)
  90 {
  91         for (auto &o: this->obstacles())
  92                 if (std::get<0>(::collide(poly, o.poly())))
  93                         return ::collide(poly, o.poly());
  94         return std::make_tuple(false, 0, 0);
  95 }
  96
  97 std::tuple<bool, unsigned int, unsigned int>
  98 RRTS::collide_steered_from(RRTNode &f)
  99 {
 100         std::vector<std::tuple<double, double>> s;
 101         s.push_back(std::make_tuple(f.x(), f.y()));
 102         for (auto &n: this->steered()) {
 103                 s.push_back(std::make_tuple(n.lfx(), n.lfy()));
 104                 s.push_back(std::make_tuple(n.lrx(), n.lry()));
 105                 s.push_back(std::make_tuple(n.rrx(), n.rry()));
 106                 s.push_back(std::make_tuple(n.rfx(), n.rfy()));
 107         }
 108         auto col = this->collide(s);
 109         auto strip_from = this->steered().size() - std::get<1>(col) / 4;
 110         if (std::get<0>(col) && strip_from > 0) {
 111                 while (strip_from-- > 0) {
 112                         this->steered().pop_back();
 113                 }
 114                 return this->collide_steered_from(f);
 115         }
 116         return col;
 117 }
 118
 119 std::tuple<bool, unsigned int, unsigned int>
 120 RRTS::collide_two_nodes(RRTNode &f, RRTNode &t)
 121 {
 122         std::vector<std::tuple<double, double>> p;
 123         p.push_back(std::make_tuple(f.lfx(), f.lfy()));
 124         p.push_back(std::make_tuple(f.lrx(), f.lry()));
 125         p.push_back(std::make_tuple(f.rrx(), f.rry()));
 126         p.push_back(std::make_tuple(f.rfx(), f.rfy()));
 127         p.push_back(std::make_tuple(t.lfx(), t.lfy()));
 128         p.push_back(std::make_tuple(t.lrx(), t.lry()));
 129         p.push_back(std::make_tuple(t.rrx(), t.rry()));
 130         p.push_back(std::make_tuple(t.rfx(), t.rfy()));
 131         return this->collide(p);
 132 }
 133
 134 double RRTS::cost_build(RRTNode &f, RRTNode &t)
 135 {
 136         double cost = 0;
 137         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
 138         return cost;
 139 }
 140
 141 double RRTS::cost_search(RRTNode &f, RRTNode &t)
 142 {
 143         double cost = 0;
 144         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
 145         return cost;
 146 }
 147
 148 void RRTS::sample()
 149 {
 150         double x = 0;
 151         double y = 0;
 152         double h = 0;
 153         switch (this->sample_dist_type()) {
 154         case 1:
 155                 x = this->udx_(this->gen_);
 156                 y = this->udy_(this->gen_);
 157                 h = this->udh_(this->gen_);
 158                 break;
 159         default:
 160                 x = this->ndx_(this->gen_);
 161                 y = this->ndy_(this->gen_);
 162                 h = this->ndh_(this->gen_);
 163         }
 164         this->samples().push_back(RRTNode());
 165         this->samples().back().x(x);
 166         this->samples().back().y(y);
 167         this->samples().back().h(h);
 168 }
 169
 170 RRTNode *RRTS::nn(RRTNode &t)
 171 {
 172         RRTNode *nn = &this->nodes().front();
 173         double cost = this->cost_search(*nn, t);
 174         for (auto &f: this->nodes()) {
 175                 if (this->cost_search(f, t) < cost) {
 176                         nn = &f;
 177                         cost = this->cost_search(f, t);
 178                 }
 179         }
 180         return nn;
 181 }
 182
 183 std::vector<RRTNode *> RRTS::nv(RRTNode &t)
 184 {
 185         std::vector<RRTNode *> nv;
 186         double cost = std::min(GAMMA(this->nodes().size()), ETA);
 187         for (auto &f: this->nodes())
 188                 if (this->cost_search(f, t) < cost)
 189                         nv.push_back(&f);
 190         return nv;
 191 }
 192
 193 int cb_rs_steer(double q[4], void *user_data)
 194 {
 195         std::vector<RRTNode> *nodes = (std::vector<RRTNode> *) user_data;
 196         RRTNode *ln = nullptr;
 197         if (nodes->size() > 0)
 198                 ln = &nodes->back();
 199         nodes->push_back(RRTNode());
 200         nodes->back().x(q[0]);
 201         nodes->back().y(q[1]);
 202         nodes->back().h(q[2]);
 203         nodes->back().sp(q[3]);
 204         if (nodes->back().sp() == 0)
 205                 nodes->back().set_t(RRTNodeType::cusp);
 206         else if (ln != nullptr && sgn(ln->sp()) != sgn(nodes->back().sp()))
 207                 ln->set_t(RRTNodeType::cusp);
 208         return 0;
 209 }
 210
 211 void RRTS::steer(RRTNode &f, RRTNode &t)
 212 {
 213         this->steered().clear();
 214         double q0[] = {f.x(), f.y(), f.h()};
 215         double q1[] = {t.x(), t.y(), t.h()};
 216         ReedsSheppStateSpace rsss(f.mtr());
 217         rsss.sample(q0, q1, 0.5, cb_rs_steer, &this->steered());
 218 }
 219
 220 void RRTS::join_steered(RRTNode *f)
 221 {
 222         while (this->steered().size() > 0) {
 223                 this->store_node(this->steered().front());
 224                 RRTNode *t = &this->nodes().back();
 225                 t->p(f);
 226                 t->c(this->cost_build(*f, *t));
 227                 this->steered().erase(this->steered().begin());
 228                 f = t;
 229         }
 230 }
 231
 232 bool RRTS::goal_found(RRTNode &f)
 233 {
 234         auto &g = this->goals().front();
 235         double cost = this->cost_build(f, g);
 236         double edist = sqrt(
 237                 pow(f.x() - g.x(), 2)
 238                 + pow(f.y() - g.y(), 2)
 239         );
 240         double adist = std::abs(f.h() - g.h());
 241         if (edist < 0.05 && adist < M_PI / 32) {
 242                 if (g.p() == nullptr || cc(f) + cost < cc(g)) {
 243                         g.p(&f);
 244                         g.c(cost);
 245                 }
 246                 return true;
 247         }
 248         return false;
 249 }
 250
 251 // RRT* procedures
 252 bool RRTS::connect()
 253 {
 254         RRTNode *t = &this->steered().front();
 255         RRTNode *f = this->nn(this->samples().back());
 256         double cost = this->cost_search(*f, *t);
 257         for (auto n: this->nv(*t)) {
 258                 if (
 259                         !std::get<0>(this->collide_two_nodes(*n, *t))
 260                         && this->cost_search(*n, *t) < cost
 261                 ) {
 262                         f = n;
 263                         cost = this->cost_search(*n, *t);
 264                 }
 265         }
 266         this->store_node(this->steered().front());
 267         t = &this->nodes().back();
 268         t->p(f);
 269         t->c(this->cost_build(*f, *t));
 270         t->set_t(RRTNodeType::connected);
 271         return true;
 272 }
 273
 274 void RRTS::rewire()
 275 {
 276         RRTNode *f = &this->nodes().back();
 277         for (auto n: this->nv(*f)) {
 278                 if (
 279                         !std::get<0>(this->collide_two_nodes(*f, *n))
 280                         && cc(*f) + this->cost_search(*f, *n) < cc(*n)
 281                 ) {
 282                         n->p(f);
 283                         n->c(this->cost_build(*f, *n));
 284                 }
 285         }
 286 }
 287
 288 // API
 289 void RRTS::init()
 290 {
 291 }
 292
 293 void RRTS::deinit()
 294 {
 295         this->nodes().clear();
 296         this->samples().clear();
 297         this->steered().clear();
 298         this->store_node(RRTNode()); // root
 299         this->icnt_ = 0;
 300         this->scnt_ = 0;
 301         this->pcnt_ = 0;
 302         this->gf_ = false;
 303 }
 304
 305 std::vector<RRTNode *> RRTS::path()
 306 {
 307         std::vector<RRTNode *> path;
 308         if (this->goals().size() == 0)
 309                 return path;
 310         RRTNode *goal = &this->goals().front();
 311         if (goal->p() == nullptr)
 312                 return path;
 313         while (goal != nullptr) {
 314                 path.push_back(goal);
 315                 goal = goal->p();
 316         }
 317         std::reverse(path.begin(), path.end());
 318         return path;
 319 }
 320
 321 bool RRTS::next()
 322 {
 323         if (this->icnt_ == 0)
 324                 this->tstart_ = std::chrono::high_resolution_clock::now();
 325         bool next = true;
 326         if (this->should_stop())
 327                 return false;
 328         this->sample();
 329         this->steer(
 330                 *this->nn(this->samples().back()),
 331                 this->samples().back()
 332         );
 333         if (std::get<0>(this->collide_steered_from(
 334                 *this->nn(this->samples().back())
 335         )))
 336                 return next;
 337         if (!this->connect())
 338                 return next;
 339         this->rewire();
 340         unsigned scnt = this->steered().size();
 341         this->join_steered(&this->nodes().back());
 342         RRTNode *just_added = &this->nodes().back();
 343         while (scnt > 0) {
 344                 scnt--;
 345                 auto &g = this->goals().front();
 346                 this->steer(*just_added, g);
 347                 if (std::get<0>(this->collide_steered_from(
 348                         *just_added
 349                 )))
 350                         continue;
 351                 this->join_steered(just_added);
 352                 this->gf(this->goal_found(this->nodes().back()));
 353                 just_added = just_added->p();
 354         }
 355         return next;
 356 }
 357
 358 void RRTS::set_sample_normal(
 359         double mx, double dx,
 360         double my, double dy,
 361         double mh, double dh
 362 )
 363 {
 364         this->ndx_ = std::normal_distribution<double>(mx, dx);
 365         this->ndy_ = std::normal_distribution<double>(my, dy);
 366         this->ndh_ = std::normal_distribution<double>(mh, dh);
 367 }
 368 void RRTS::set_sample_uniform(
 369         double xmin, double xmax,
 370         double ymin, double ymax,
 371         double hmin, double hmax
 372 )
 373 {
 374         this->udx_ = std::uniform_real_distribution<double>(xmin,xmax);
 375         this->udy_ = std::uniform_real_distribution<double>(ymin,ymax);
 376         this->udh_ = std::uniform_real_distribution<double>(hmin,hmax);
 377 }
 378 void RRTS::set_sample(
 379         double x1, double x2,
 380         double y1, double y2,
 381         double h1, double h2
 382 )
 383 {
 384         switch (this->sample_dist_type()) {
 385         case 1:
 386                 x1 += this->nodes().front().x();
 387                 x2 += this->nodes().front().x();
 388                 y1 += this->nodes().front().y();
 389                 y2 += this->nodes().front().y();
 390                 this->set_sample_uniform(x1, x2, y1, y2, h1, h2);
 391                 break;
 392         default:
 393                 this->set_sample_normal(x1, x2, y1, y2, h1, h2);
 394         }
 395 }
 396
 397 Json::Value RRTS::json()
 398 {
 399         Json::Value jvo;
 400         {
 401                 jvo["time"] = this->scnt();
 402         }
 403         {
 404                 jvo["iterations"] = this->icnt();
 405         }
 406         {
 407                 jvo["init"][0] = this->nodes().front().x();
 408                 jvo["init"][1] = this->nodes().front().y();
 409                 jvo["init"][2] = this->nodes().front().h();
 410         }
 411         {
 412                 if (this->path().size() > 0) {
 413                         jvo["cost"] = cc(*this->path().back());
 414                         jvo["goal"][0] = this->path().back()->x();
 415                         jvo["goal"][1] = this->path().back()->y();
 416                         jvo["goal"][2] = this->path().back()->h();
 417                 }
 418         }
 419         {
 420                 unsigned int cu = 0;
 421                 unsigned int co = 0;
 422                 unsigned int pcnt = 0;
 423                 for (auto n: this->path()) {
 424                         jvo["path"][pcnt][0] = n->x();
 425                         jvo["path"][pcnt][1] = n->y();
 426                         jvo["path"][pcnt][2] = n->h();
 427                         if (n->t(RRTNodeType::cusp))
 428                                 cu++;
 429                         if (n->t(RRTNodeType::connected))
 430                                 co++;
 431                         pcnt++;
 432                 }
 433                 jvo["cusps-in-path"] = cu;
 434                 jvo["connecteds-in-path"] = co;
 435         }
 436         {
 437                 unsigned int gcnt = 0;
 438                 for (auto g: this->goals()) {
 439                         jvo["goals"][gcnt][0] = g.x();
 440                         jvo["goals"][gcnt][1] = g.y();
 441                         jvo["goals"][gcnt][2] = g.h();
 442                         gcnt++;
 443                 }
 444         }
 445         {
 446                 unsigned int ocnt = 0;
 447                 for (auto o: this->obstacles()) {
 448                         unsigned int ccnt = 0;
 449                         for (auto c: o.poly()) {
 450                                 jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
 451                                 jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
 452                                 ccnt++;
 453                         }
 454                         ocnt++;
 455                 }
 456         }
 457         {
 458                 jvo["nodes"] = (unsigned int) this->nodes().size();
 459         }
 460         //{
 461         //        unsigned int ncnt = 0;
 462         //        for (auto n: this->nodes()) {
 463         //                jvo["nodes_x"][ncnt] = n.x();
 464         //                jvo["nodes_y"][ncnt] = n.y();
 465         //                //jvo["nodes_h"][ncnt] = n.h();
 466         //                ncnt++;
 467         //        }
 468         //}
 469         return jvo;
 470 }
 471
 472 void RRTS::json(Json::Value jvi)
 473 {
 474         assert(jvi["init"] != Json::nullValue);
 475         assert(jvi["goal"] != Json::nullValue);
 476         assert(jvi["goals"] != Json::nullValue);
 477         assert(jvi["obst"] != Json::nullValue);
 478
 479         this->nodes().front().x(jvi["init"][0].asDouble());
 480         this->nodes().front().y(jvi["init"][1].asDouble());
 481         this->nodes().front().h(jvi["init"][2].asDouble());
 482         {
 483                 RRTNode tmp_node;
 484                 tmp_node.x(jvi["goal"][0].asDouble());
 485                 tmp_node.y(jvi["goal"][1].asDouble());
 486                 tmp_node.h(jvi["goal"][2].asDouble());
 487                 this->goals().push_back(tmp_node);
 488                 for (auto g: jvi["goals"]) {
 489                         tmp_node.x(g[0].asDouble());
 490                         tmp_node.y(g[1].asDouble());
 491                         tmp_node.h(g[2].asDouble());
 492                         this->goals().push_back(tmp_node);
 493                 }
 494         }
 495         {
 496                 Obstacle tmp_obstacle;
 497                 for (auto o: jvi["obst"]) {
 498                         tmp_obstacle.poly().clear();
 499                         for (auto c: o) {
 500                                 double tmp_x = c[0].asDouble();
 501                                 double tmp_y = c[1].asDouble();
 502                                 auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
 503                                 tmp_obstacle.poly().push_back(tmp_tuple);
 504                         }
 505                         this->obstacles().push_back(tmp_obstacle);
 506                 }
 507         }
 508         {
 509                 double edist_init_goal = sqrt(
 510                         pow(
 511                                 this->nodes().front().x()
 512                                 - this->goals().front().x(),
 513                                 2
 514                         )
 515                         + pow(
 516                                 this->nodes().front().y()
 517                                 - this->goals().front().y(),
 518                                 2
 519                         )
 520                 );
 521                 this->set_sample(
 522                         this->nodes().front().x(), edist_init_goal,
 523                         this->nodes().front().y(), edist_init_goal,
 524                         0, 2 * M_PI
 525                 );
 526         }
 527 }
 528
 529 RRTS::RRTS()
 530         : gen_(std::random_device{}())
 531 {
 532         this->goals().reserve(100);
 533         this->nodes().reserve(4000000);
 534         this->samples().reserve(1000);
 535         this->steered().reserve(20000);
 536         this->store_node(RRTNode()); // root
 537 }
 538
 539 double cc(RRTNode &t)
 540 {
 541         RRTNode *n = &t;
 542         double cost = 0;
 543         while (n != nullptr) {
 544                 cost += n->c();
 545                 n = n->p();
 546         }
 547         return cost;
 548 }