src/rrts.cc

   1 #include <algorithm>
   2 #include "rrts.h"
   3
   4 #include "reeds_shepp.h"
   5
   6 template <typename T> int sgn(T val) {
   7         return (T(0) < val) - (val < T(0));
   8 }
   9
  10 double edist(RRTNode const& n1, RRTNode const& n2)
  11 {
  12     auto dx = n2.x() - n1.x();
  13     auto dy = n2.y() - n1.y();
  14     return sqrt(dx*dx + dy*dy);
  15 }
  16 double edist(RRTNode const* n1, RRTNode const* n2)
  17 {
  18     return edist(*n1, *n2);
  19 }
  20
  21 RRTNode::RRTNode()
  22 {
  23 }
  24
  25 RRTNode::RRTNode(const BicycleCar &bc)
  26 {
  27     this->x(bc.x());
  28     this->y(bc.y());
  29     this->h(bc.h());
  30     this->sp(bc.sp());
  31     this->st(bc.st());
  32 }
  33
  34 bool RRTNode::operator==(const RRTNode& n)
  35 {
  36         if (this == &n)
  37                 return true;
  38         return false;
  39 }
  40
  41 Obstacle::Obstacle()
  42 {
  43 }
  44
  45 double RRTS::elapsed()
  46 {
  47         std::chrono::duration<double> dt;
  48         dt = std::chrono::duration_cast<std::chrono::duration<double>>(
  49                 std::chrono::high_resolution_clock::now()
  50                 - this->tstart_
  51         );
  52         this->scnt_ = dt.count();
  53         return this->scnt_;
  54 }
  55
  56 void RRTS::log_path_cost()
  57 {
  58         this->log_path_cost_.push_back(cc(this->goals().front()));
  59         this->log_path_time_ += 0.1;
  60 }
  61
  62 bool RRTS::should_stop()
  63 {
  64         // the following counters must be updated, do not comment
  65         this->icnt_++;
  66         this->elapsed();
  67         // current iteration stop conditions
  68         if (this->should_finish()) return true;
  69         if (this->should_break()) return true;
  70         // but continue by default
  71         return false;
  72 }
  73
  74 bool RRTS::should_finish()
  75 {
  76         // decide finish conditions (maybe comment some lines)
  77         //if (this->icnt_ > 999) return true;
  78         if (this->scnt_ > 2) return true;
  79         if (this->finishit) return true;
  80         //if (this->gf()) return true;
  81         // but continue by default
  82         return false;
  83 }
  84
  85 bool RRTS::should_break()
  86 {
  87         // decide break conditions (maybe comment some lines)
  88         //if (this->scnt_ - this->pcnt_ > 2) return true;
  89         // but continue by default
  90         return false;
  91 }
  92
  93 bool RRTS::should_continue()
  94 {
  95         // decide the stop conditions (maybe comment some lines)
  96         // it is exact opposite of `should_stop`
  97         //if (this->icnt_ > 999) return false;
  98         if (this->scnt_ > 10) return false;
  99         if (this->gf()) return false;
 100         // and reset pause counter if should continue
 101         this->pcnt_ = this->scnt_;
 102         return true;
 103 }
 104
 105 void RRTS::store_node(RRTNode n)
 106 {
 107         this->nodes().push_back(n);
 108 }
 109
 110 // RRT procedures
 111 std::tuple<bool, unsigned int, unsigned int>
 112 RRTS::collide(std::vector<std::tuple<double, double>> &poly)
 113 {
 114         for (auto &o: this->obstacles())
 115                 if (std::get<0>(::collide(poly, o.poly())))
 116                         return ::collide(poly, o.poly());
 117         return std::make_tuple(false, 0, 0);
 118 }
 119
 120 std::tuple<bool, unsigned int, unsigned int>
 121 RRTS::collide_steered_from(RRTNode &f)
 122 {
 123         auto fbc = BicycleCar();
 124         fbc.x(f.x());
 125         fbc.y(f.y());
 126         fbc.h(f.h());
 127         std::vector<std::tuple<double, double>> s;
 128         s.push_back(std::make_tuple(fbc.x(), fbc.y()));
 129         for (auto &n: this->steered()) {
 130                 auto nbc = BicycleCar();
 131                 nbc.x(n.x());
 132                 nbc.y(n.y());
 133                 nbc.h(n.h());
 134                 s.push_back(std::make_tuple(nbc.lfx(), nbc.lfy()));
 135                 s.push_back(std::make_tuple(nbc.lrx(), nbc.lry()));
 136                 s.push_back(std::make_tuple(nbc.rrx(), nbc.rry()));
 137                 s.push_back(std::make_tuple(nbc.rfx(), nbc.rfy()));
 138         }
 139         auto col = this->collide(s);
 140         auto strip_from = this->steered().size() - std::get<1>(col) / 4;
 141         if (std::get<0>(col) && strip_from > 0) {
 142                 while (strip_from-- > 0) {
 143                         this->steered().pop_back();
 144                 }
 145                 return this->collide_steered_from(f);
 146         }
 147         return col;
 148 }
 149 std::tuple<bool, unsigned int, unsigned int>
 150 RRTS::collide_tmp_steered_from(RRTNode &f)
 151 {
 152         return std::make_tuple(false, 0, 0);
 153 }
 154
 155 std::tuple<bool, unsigned int, unsigned int>
 156 RRTS::collide_two_nodes(RRTNode &f, RRTNode &t)
 157 {
 158         auto fbc = BicycleCar();
 159         fbc.x(f.x());
 160         fbc.y(f.y());
 161         fbc.h(f.h());
 162         auto tbc = BicycleCar();
 163         tbc.x(f.x());
 164         tbc.y(f.y());
 165         tbc.h(f.h());
 166         std::vector<std::tuple<double, double>> p;
 167         p.push_back(std::make_tuple(fbc.lfx(), fbc.lfy()));
 168         p.push_back(std::make_tuple(fbc.lrx(), fbc.lry()));
 169         p.push_back(std::make_tuple(fbc.rrx(), fbc.rry()));
 170         p.push_back(std::make_tuple(fbc.rfx(), fbc.rfy()));
 171         p.push_back(std::make_tuple(tbc.lfx(), tbc.lfy()));
 172         p.push_back(std::make_tuple(tbc.lrx(), tbc.lry()));
 173         p.push_back(std::make_tuple(tbc.rrx(), tbc.rry()));
 174         p.push_back(std::make_tuple(tbc.rfx(), tbc.rfy()));
 175         return this->collide(p);
 176 }
 177
 178 double RRTS::cost_build(RRTNode &f, RRTNode &t)
 179 {
 180         double cost = 0;
 181         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
 182         return cost;
 183 }
 184
 185 double RRTS::cost_search(RRTNode &f, RRTNode &t)
 186 {
 187         double cost = 0;
 188         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
 189         return cost;
 190 }
 191
 192 void RRTS::sample()
 193 {
 194         double x = 0;
 195         double y = 0;
 196         double h = 0;
 197         switch (this->sample_dist_type()) {
 198         case 1: // uniform
 199                 x = this->udx_(this->gen_);
 200                 y = this->udy_(this->gen_);
 201                 h = this->udh_(this->gen_);
 202                 break;
 203         case 2: // uniform circle
 204         {
 205                 // see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
 206                 double R = sqrt(
 207                         pow(
 208                                 this->nodes().front().x()
 209                                 - this->goals().front().x(),
 210                                 2
 211                         )
 212                         + pow(
 213                                 this->nodes().front().y()
 214                                 - this->goals().front().y(),
 215                                 2
 216                         )
 217                 );
 218                 double a = atan2(
 219                         this->goals().front().y() - this->nodes().front().y(),
 220                         this->goals().front().x() - this->nodes().front().x()
 221                 );
 222                 double cx = this->goals().front().x() - R/2 * cos(a);
 223                 double cy = this->goals().front().y() - R/2 * sin(a);
 224                 double r = R * sqrt(this->udx_(this->gen_));
 225                 double theta = this->udy_(this->gen_) * 2 * M_PI;
 226                 x = cx + r * cos(theta);
 227                 y = cy + r * sin(theta);
 228                 h = this->udh_(this->gen_);
 229         }
 230                 break;
 231         case 3: {
 232                 if (
 233                         this->steered1_.size() == 0
 234                         && this->steered2_.size() == 0
 235                 ) {
 236                         x = this->nodes().front().x();
 237                         y = this->nodes().front().y();
 238                         h = this->nodes().front().h();
 239                 } else {
 240                         this->udi1_ = std::uniform_int_distribution<unsigned int>(
 241                                 0,
 242                                 this->steered1_.size() - 1
 243                         );
 244                         this->udi2_ = std::uniform_int_distribution<unsigned int>(
 245                                 0,
 246                                 this->steered2_.size() - 1
 247                         );
 248                         auto ind1 = this->udi1_(this->gen_);
 249                         auto ind2 = this->udi2_(this->gen_);
 250                         if (
 251                                 this->steered2_.size() == 0
 252                         ) {
 253                                 auto n1 = this->steered1_[ind1];
 254                                 x = n1->x();
 255                                 y = n1->y();
 256                                 h = n1->h();
 257                         } else if (
 258                                 this->steered1_.size() == 0
 259                         ) {
 260                                 auto n2 = this->steered2_[ind2];
 261                                 x = n2->x();
 262                                 y = n2->y();
 263                                 h = n2->h();
 264                         } else {
 265                                 auto n1 = this->steered1_[ind1];
 266                                 auto n2 = this->steered2_[ind2];
 267                                 auto which = this->udx_(this->gen_);
 268                                 if (which > 0.5) {
 269                                         x = n1->x();
 270                                         y = n1->y();
 271                                         h = n1->h();
 272                                 } else {
 273                                         x = n2->x();
 274                                         y = n2->y();
 275                                         h = n2->h();
 276                                 }
 277                         }
 278                 }
 279                 break;
 280                 }
 281         default: // normal
 282                 x = this->ndx_(this->gen_);
 283                 y = this->ndy_(this->gen_);
 284                 h = this->ndh_(this->gen_);
 285         }
 286         this->samples().push_back(RRTNode());
 287         this->samples().back().x(x);
 288         this->samples().back().y(y);
 289         this->samples().back().h(h);
 290 }
 291
 292 RRTNode *RRTS::nn(RRTNode &t)
 293 {
 294         RRTNode *nn = &this->nodes().front();
 295         double cost = this->cost_search(*nn, t);
 296         for (auto &f: this->nodes()) {
 297                 if (this->cost_search(f, t) < cost) {
 298                         nn = &f;
 299                         cost = this->cost_search(f, t);
 300                 }
 301         }
 302         return nn;
 303 }
 304
 305 std::vector<RRTNode *> RRTS::nv(RRTNode &t)
 306 {
 307         std::vector<RRTNode *> nv;
 308         double cost = std::min(GAMMA(this->nodes().size()), ETA);
 309         for (auto &f: this->nodes())
 310                 if (this->cost_search(f, t) < cost)
 311                         nv.push_back(&f);
 312         return nv;
 313 }
 314
 315 int cb_rs_steer(double q[4], void *user_data)
 316 {
 317         std::vector<RRTNode> *nodes = (std::vector<RRTNode> *) user_data;
 318         nodes->push_back(RRTNode());
 319         nodes->back().x(q[0]);
 320         nodes->back().y(q[1]);
 321         nodes->back().h(q[2]);
 322         nodes->back().sp(q[3]);
 323         if (nodes->back().sp() == 0) {
 324                 nodes->back().set_t(RRTNodeType::cusp);
 325         } else if (nodes->size() >= 2) {
 326                 RRTNode* lln = nodes->back().p();
 327                 RRTNode* ln = &nodes->back();
 328                 if (lln != nullptr && ln != nullptr && sgn(lln->sp()) != sgn(ln->sp()))
 329                         ln->set_t(RRTNodeType::cusp);
 330         }
 331         return 0;
 332 }
 333
 334 void RRTS::steer(RRTNode &f, RRTNode &t)
 335 {
 336         this->steered().clear();
 337         double q0[] = {f.x(), f.y(), f.h()};
 338         double q1[] = {t.x(), t.y(), t.h()};
 339         ReedsSheppStateSpace rsss(this->bc.mtr());
 340         rsss.sample(q0, q1, 0.2, cb_rs_steer, &this->steered());
 341 }
 342 void RRTS::tmp_steer(RRTNode &f, RRTNode &t)
 343 {
 344     this->tmp_steered_.clear();
 345     double q0[] = {f.x(), f.y(), f.h()};
 346     double q1[] = {t.x(), t.y(), t.h()};
 347     ReedsSheppStateSpace rsss(this->bc.mtr());
 348     rsss.sample(q0, q1, 0.2, cb_rs_steer, &this->tmp_steered_);
 349 }
 350
 351 void RRTS::steer1(RRTNode &f, RRTNode &t)
 352 {
 353     return this->steer(f, t);
 354 }
 355
 356 void RRTS::steer2(RRTNode &f, RRTNode &t)
 357 {
 358     return this->steer(f, t);
 359 }
 360
 361 void RRTS::join_steered(RRTNode *f)
 362 {
 363         while (this->steered().size() > 0) {
 364                 this->store_node(this->steered().front());
 365                 RRTNode *t = &this->nodes().back();
 366                 t->p(f);
 367                 t->c(this->cost_build(*f, *t));
 368                 this->steered().erase(this->steered().begin());
 369                 f = t;
 370         }
 371 }
 372 void RRTS::join_tmp_steered(RRTNode *f)
 373 {
 374         while (this->tmp_steered_.size() > 0) {
 375                 this->store_node(this->tmp_steered_.front());
 376                 RRTNode *t = &this->nodes().back();
 377                 t->p(f);
 378                 t->c(this->cost_build(*f, *t));
 379                 this->tmp_steered_.erase(this->tmp_steered_.begin());
 380                 f = t;
 381         }
 382 }
 383
 384 bool RRTS::goal_found(RRTNode &f)
 385 {
 386         auto &g = this->goals().front();
 387         double cost = this->cost_build(f, g);
 388         double edist = sqrt(
 389                 pow(f.x() - g.x(), 2)
 390                 + pow(f.y() - g.y(), 2)
 391         );
 392         double adist = std::abs(f.h() - g.h());
 393         if (edist < 0.05 && adist < M_PI / 32) {
 394                 if (g.p() == nullptr || cc(f) + cost < cc(g)) {
 395                         g.p(&f);
 396                         g.c(cost);
 397                 }
 398                 return true;
 399         }
 400         return false;
 401 }
 402
 403 // RRT* procedures
 404 bool RRTS::connect()
 405 {
 406         RRTNode *t = &this->steered().front();
 407         RRTNode *f = this->nn(this->samples().back());
 408         double cost = this->cost_search(*f, *t);
 409         for (auto n: this->nv(*t)) {
 410                 if (
 411                         !std::get<0>(this->collide_two_nodes(*n, *t))
 412                         && this->cost_search(*n, *t) < cost
 413                 ) {
 414                         f = n;
 415                         cost = this->cost_search(*n, *t);
 416                 }
 417         }
 418         // steer from f->t and then continue with the steered.
 419         this->tmp_steer(*f, *t);
 420         if (this->tmp_steered_.size() > 0) {
 421                 auto col = this->collide_tmp_steered_from(*f);
 422                 if (std::get<0>(col))
 423                         return false;
 424                 this->join_tmp_steered(f);
 425                 f = &this->nodes().back();
 426         }
 427         if (edist(f, t) > 0.2)
 428                 return false;
 429         // cont.
 430         this->store_node(this->steered().front());
 431         t = &this->nodes().back();
 432         t->p(f);
 433         t->c(this->cost_build(*f, *t));
 434         t->set_t(RRTNodeType::connected);
 435         return true;
 436 }
 437
 438 void RRTS::rewire()
 439 {
 440         RRTNode *f = &this->nodes().back();
 441         for (auto n: this->nv(*f)) {
 442                 if (
 443                         !std::get<0>(this->collide_two_nodes(*f, *n))
 444                         && cc(*f) + this->cost_search(*f, *n) < cc(*n)
 445                 ) {
 446                         n->p(f);
 447                         n->c(this->cost_build(*f, *n));
 448                 }
 449         }
 450 }
 451
 452 // API
 453 void RRTS::init()
 454 {
 455 }
 456
 457 void RRTS::deinit()
 458 {
 459         this->nodes().clear();
 460         this->samples().clear();
 461         this->steered().clear();
 462         this->store_node(RRTNode()); // root
 463         this->icnt_ = 0;
 464         this->scnt_ = 0;
 465         this->pcnt_ = 0;
 466         this->gf_ = false;
 467 }
 468
 469 std::vector<RRTNode *> RRTS::path()
 470 {
 471         std::vector<RRTNode *> path;
 472         if (this->goals().size() == 0)
 473                 return path;
 474         RRTNode *goal = &this->goals().back();
 475         if (goal->p() == nullptr)
 476                 return path;
 477         while (goal != nullptr) {
 478                 path.push_back(goal);
 479                 goal = goal->p();
 480         }
 481         std::reverse(path.begin(), path.end());
 482         return path;
 483 }
 484
 485 bool RRTS::next()
 486 {
 487         if (this->icnt_ == 0)
 488                 this->tstart_ = std::chrono::high_resolution_clock::now();
 489         bool next = true;
 490         if (this->scnt_ > this->log_path_time_)
 491             this->log_path_cost();
 492         if (this->should_stop())
 493                 return false;
 494         if (this->samples().size() == 0) {
 495                 this->samples().push_back(RRTNode());
 496                 this->samples().back().x(this->goals().front().x());
 497                 this->samples().back().y(this->goals().front().y());
 498                 this->samples().back().h(this->goals().front().h());
 499         } else {
 500                 this->sample();
 501         }
 502         this->steer1(
 503                 *this->nn(this->samples().back()),
 504                 this->samples().back()
 505         );
 506         if (this->steered().size() == 0)
 507                 return next;
 508         auto col = this->collide_steered_from(
 509                 *this->nn(this->samples().back())
 510         );
 511         if (std::get<0>(col)) {
 512                 auto rcnt = this->steered().size() - std::get<1>(col);
 513                 while (rcnt-- > 0) {
 514                         this->steered().pop_back();
 515                 }
 516         }
 517         if (!this->connect())
 518                 return next;
 519         this->rewire();
 520         unsigned scnt = this->steered().size();
 521         this->join_steered(&this->nodes().back());
 522         RRTNode *just_added = &this->nodes().back();
 523         while (scnt > 0) {
 524                 // store all the steered1 nodes
 525                 this->steered1_.push_back(just_added);
 526                 scnt--;
 527                 auto &g = this->goals().front();
 528                 this->steer2(*just_added, g);
 529                 auto col = this->collide_steered_from(*just_added);
 530                 if (std::get<0>(col)) {
 531                         auto rcnt = this->steered().size() - std::get<1>(col);
 532                         while (rcnt-- > 0) {
 533                                 this->steered().pop_back();
 534                         }
 535                 }
 536                 this->join_steered(just_added);
 537                 // store all the steered2 nodes
 538                 RRTNode* jap = &this->nodes().back();
 539                 while (jap != just_added) {
 540                         this->steered2_.push_back(jap);
 541                         jap = jap->p();
 542                 }
 543                 this->gf(this->goal_found(this->nodes().back()));
 544                 just_added = just_added->p();
 545         }
 546         return next;
 547 }
 548
 549 void RRTS::set_sample_normal(
 550         double mx, double dx,
 551         double my, double dy,
 552         double mh, double dh
 553 )
 554 {
 555         this->ndx_ = std::normal_distribution<double>(mx, dx);
 556         this->ndy_ = std::normal_distribution<double>(my, dy);
 557         this->ndh_ = std::normal_distribution<double>(mh, dh);
 558 }
 559 void RRTS::set_sample_uniform(
 560         double xmin, double xmax,
 561         double ymin, double ymax,
 562         double hmin, double hmax
 563 )
 564 {
 565         this->udx_ = std::uniform_real_distribution<double>(xmin,xmax);
 566         this->udy_ = std::uniform_real_distribution<double>(ymin,ymax);
 567         this->udh_ = std::uniform_real_distribution<double>(hmin,hmax);
 568 }
 569 void RRTS::set_sample_uniform_circle()
 570 {
 571         this->udx_ = std::uniform_real_distribution<double>(0, 1);
 572         this->udy_ = std::uniform_real_distribution<double>(0, 1);
 573         this->udh_ = std::uniform_real_distribution<double>(0, 2 * M_PI);
 574 }
 575 void RRTS::set_sample(
 576         double x1, double x2,
 577         double y1, double y2,
 578         double h1, double h2
 579 )
 580 {
 581         switch (this->sample_dist_type()) {
 582         case 1: // uniform
 583                 x1 += this->nodes().front().x();
 584                 x2 += this->nodes().front().x();
 585                 y1 += this->nodes().front().y();
 586                 y2 += this->nodes().front().y();
 587                 this->set_sample_uniform(x1, x2, y1, y2, h1, h2);
 588                 break;
 589         case 2: // uniform circle
 590                 this->set_sample_uniform_circle();
 591                 break;
 592         case 3: // uniform index of node in nodes
 593                 this->set_sample_uniform_circle();
 594                 break;
 595         default: // normal
 596                 this->set_sample_normal(x1, x2, y1, y2, h1, h2);
 597         }
 598 }
 599
 600 Json::Value RRTS::json()
 601 {
 602         Json::Value jvo;
 603         {
 604                 jvo["time"] = this->scnt();
 605         }
 606         {
 607                 jvo["iterations"] = this->icnt();
 608         }
 609         {
 610                 jvo["init"][0] = this->nodes().front().x();
 611                 jvo["init"][1] = this->nodes().front().y();
 612                 jvo["init"][2] = this->nodes().front().h();
 613         }
 614         {
 615                 jvo["path_cost_before_opt"] = this->path_cost_before_opt_;
 616         }
 617         {
 618                 if (this->path().size() > 0) {
 619                         jvo["cost"] = cc(*this->path().back());
 620                         jvo["entry"][0] = this->goals().front().x();
 621                         jvo["entry"][1] = this->goals().front().y();
 622                         jvo["entry"][2] = this->goals().front().h();
 623                         jvo["goal"][0] = this->goals().back().x();
 624                         jvo["goal"][1] = this->goals().back().y();
 625                         jvo["goal"][2] = this->goals().back().h();
 626                 }
 627         }
 628         {
 629                 unsigned int cu = 0;
 630                 unsigned int co = 0;
 631                 unsigned int pcnt = 0;
 632                 for (auto n: this->path()) {
 633                         jvo["path"][pcnt][0] = n->x();
 634                         jvo["path"][pcnt][1] = n->y();
 635                         jvo["path"][pcnt][2] = n->h();
 636                         if (n->t(RRTNodeType::cusp))
 637                                 cu++;
 638                         if (n->t(RRTNodeType::connected))
 639                                 co++;
 640                         pcnt++;
 641                 }
 642                 jvo["cusps-in-path"] = cu;
 643                 jvo["connecteds-in-path"] = co;
 644         }
 645         {
 646                 unsigned int gcnt = 0;
 647                 for (auto g: this->goals()) {
 648                         jvo["goals"][gcnt][0] = g.x();
 649                         jvo["goals"][gcnt][1] = g.y();
 650                         jvo["goals"][gcnt][2] = g.h();
 651                         gcnt++;
 652                 }
 653         }
 654         {
 655                 unsigned int ocnt = 0;
 656                 for (auto o: this->obstacles()) {
 657                         unsigned int ccnt = 0;
 658                         for (auto c: o.poly()) {
 659                                 jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
 660                                 jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
 661                                 ccnt++;
 662                         }
 663                         ocnt++;
 664                 }
 665         }
 666         {
 667                 jvo["nodes"] = (unsigned int) this->nodes().size();
 668         }
 669         {
 670                 unsigned int cnt = 0;
 671                 for (auto i: this->log_path_cost_)
 672                         jvo["log_path_cost"][cnt++] = i;
 673         }
 674         //{
 675         //        unsigned int ncnt = 0;
 676         //        for (auto n: this->nodes()) {
 677         //                jvo["nodes_x"][ncnt] = n.x();
 678         //                jvo["nodes_y"][ncnt] = n.y();
 679         //                //jvo["nodes_h"][ncnt] = n.h();
 680         //                ncnt++;
 681         //        }
 682         //}
 683         //{
 684         //        unsigned int ncnt = 0;
 685         //        for (auto n: this->steered1_) {
 686         //                jvo["steered1_x"][ncnt] = n->x();
 687         //                jvo["steered1_y"][ncnt] = n->y();
 688         //                //jvo["nodes_h"][ncnt] = n.h();
 689         //                ncnt++;
 690         //        }
 691         //        ncnt = 0;
 692         //        for (auto n: this->steered2_) {
 693         //                jvo["steered2_x"][ncnt] = n->x();
 694         //                jvo["steered2_y"][ncnt] = n->y();
 695         //                //jvo["nodes_h"][ncnt] = n.h();
 696         //                ncnt++;
 697         //        }
 698         //}
 699         return jvo;
 700 }
 701
 702 void RRTS::json(Json::Value jvi)
 703 {
 704         assert(jvi["init"] != Json::nullValue);
 705         assert(jvi["goals"] != Json::nullValue);
 706         assert(jvi["obst"] != Json::nullValue);
 707
 708         this->nodes().front().x(jvi["init"][0].asDouble());
 709         this->nodes().front().y(jvi["init"][1].asDouble());
 710         this->nodes().front().h(jvi["init"][2].asDouble());
 711         {
 712                 RRTNode* gp = nullptr;
 713                 if (jvi["entry"] != Json::nullValue) {
 714                         this->entry_set = true;
 715                         this->entry.x = jvi["entry"][0].asDouble();
 716                         this->entry.y = jvi["entry"][1].asDouble();
 717                         this->entry.b = jvi["entry"][2].asDouble();
 718                         this->entry.e = jvi["entry"][3].asDouble();
 719                         RRTNode tmp_node;
 720                         tmp_node.x(this->entry.x);
 721                         tmp_node.y(this->entry.y);
 722                         tmp_node.h((this->entry.b + this->entry.e) / 2.0);
 723                         this->goals().push_back(tmp_node);
 724                         this->goals().back().p(gp);
 725                         gp = &this->goals().back();
 726                 }
 727                 for (auto g: jvi["goals"]) {
 728                         RRTNode tmp_node;
 729                         tmp_node.x(g[0].asDouble());
 730                         tmp_node.y(g[1].asDouble());
 731                         tmp_node.h(g[2].asDouble());
 732                         this->goals().push_back(tmp_node);
 733                         this->goals().back().p(gp);
 734                         gp = &this->goals().back();
 735                 }
 736                 this->goals().front().set_t(RRTNodeType::cusp);
 737                 this->goals().back().set_t(RRTNodeType::cusp);
 738         }
 739         {
 740                 Obstacle tmp_obstacle;
 741                 for (auto o: jvi["obst"]) {
 742                         tmp_obstacle.poly().clear();
 743                         for (auto c: o) {
 744                                 double tmp_x = c[0].asDouble();
 745                                 double tmp_y = c[1].asDouble();
 746                                 auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
 747                                 tmp_obstacle.poly().push_back(tmp_tuple);
 748                         }
 749                         this->obstacles().push_back(tmp_obstacle);
 750                 }
 751         }
 752         {
 753                 double edist_init_goal = sqrt(
 754                         pow(
 755                                 this->nodes().front().x()
 756                                 - this->goals().front().x(),
 757                                 2
 758                         )
 759                         + pow(
 760                                 this->nodes().front().y()
 761                                 - this->goals().front().y(),
 762                                 2
 763                         )
 764                 );
 765                 this->set_sample(
 766                         this->nodes().front().x(), edist_init_goal,
 767                         this->nodes().front().y(), edist_init_goal,
 768                         0, 2 * M_PI
 769                 );
 770         }
 771 }
 772
 773 RRTS::RRTS()
 774         : gen_(std::random_device{}())
 775 {
 776         this->goals().reserve(100);
 777         this->nodes().reserve(4000000);
 778         this->samples().reserve(1000);
 779         this->steered().reserve(20000);
 780         this->store_node(RRTNode()); // root
 781 }
 782
 783 double cc(RRTNode &t)
 784 {
 785         RRTNode *n = &t;
 786         double cost = 0;
 787         while (n != nullptr) {
 788                 cost += n->c();
 789                 n = n->p();
 790         }
 791         return cost;
 792 }