rrts/src/rrts.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <algorithm>
   8 #include <cassert>
   9 #include "rrts.hh"
  10
  11 #ifndef USE_RRTS
  12 #define USE_RRTS 0  // TODO improve, this solution isn't clear.
  13 #endif
  14
  15 namespace rrts {
  16
  17 void
  18 Ter::start()
  19 {
  20         this->_tstart = std::chrono::high_resolution_clock::now();
  21 }
  22
  23 double
  24 Ter::scnt() const
  25 {
  26         auto t = std::chrono::high_resolution_clock::now() - this->_tstart;
  27         auto d = std::chrono::duration_cast<std::chrono::seconds>(t);
  28         return d.count();
  29 }
  30
  31 double
  32 RRTNode::c() const
  33 {
  34         return this->_c;
  35 }
  36
  37 void
  38 RRTNode::c(double c)
  39 {
  40         assert(this->_p != nullptr);
  41         this->_c = c;
  42         this->_cc = this->_p->cc() + c;
  43 }
  44
  45 double
  46 RRTNode::cc() const
  47 {
  48         return this->_cc;
  49 }
  50
  51 RRTNode*
  52 RRTNode::p() const
  53 {
  54         return this->_p;
  55 }
  56
  57 void
  58 RRTNode::p(RRTNode& p, bool connecting_goal)
  59 {
  60         assert(this != &p);
  61         if (!connecting_goal) {
  62                 assert(!(std::abs(p.x() - this->x()) < 1e-3
  63                         && std::abs(p.y() - this->y()) < 1e-3
  64                         && std::abs(p.h() - this->h()) < 1e-3));
  65         }
  66         this->_p = &p;
  67         this->p_is_cusp(this->would_be_cusp_if_parent(p));
  68         this->cusp_cnt(p);
  69 }
  70
  71 void
  72 RRTNode::p(RRTNode& p)
  73 {
  74         return this->p(p, false);
  75 }
  76
  77 unsigned int
  78 RRTNode::cusp_cnt() const
  79 {
  80         return this->_cusp_cnt;
  81 }
  82
  83 void
  84 RRTNode::cusp_cnt(RRTNode const& p)
  85 {
  86         this->_cusp_cnt = p.cusp_cnt();
  87         if (this->_p_is_cusp) {
  88                 this->_cusp_cnt++;
  89         }
  90 }
  91
  92 int
  93 RRTNode::st(void)
  94 {
  95         return this->_segment_type;
  96 }
  97
  98 void
  99 RRTNode::st(int st)
 100 {
 101         this->_segment_type = st;
 102 }
 103
 104 bool
 105 RRTNode::p_is_cusp(void) const
 106 {
 107         return this->_p_is_cusp;
 108 }
 109
 110 void
 111 RRTNode::p_is_cusp(bool isit)
 112 {
 113         this->_p_is_cusp = isit;
 114 }
 115
 116 bool
 117 RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
 118 {
 119         if (std::abs(p.sp()) < 1e-3) {
 120                 // It should not be possible to have two zero speed nodes next
 121                 // to each other. In practice, this sometimes happens.
 122                 //assert(std::abs(this->sp()) > 1e-3);
 123                 if (p.p()) {
 124                         if (sgn(p.p()->sp()) != sgn(this->sp())) {
 125                                 return true;
 126                         } else {
 127                                 return false;
 128                         }
 129                 } else {
 130                         return true; // only root has no parent and it is cusp
 131                 }
 132         } else {
 133                 if (std::abs(this->sp()) < 1e-3) {
 134                         return false; // this is cusp, not the parent
 135                 } else if (sgn(p.sp()) != sgn(this->sp())) {
 136                         return true;
 137                 } else {
 138                         return false;
 139                 }
 140         }
 141 }
 142
 143 bool
 144 RRTNode::operator==(RRTNode const& n)
 145 {
 146         return this == &n;
 147 }
 148
 149 void
 150 RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
 151 {
 152         assert(this->_path.size() == 0);
 153         while (g != nullptr) {
 154                 this->_path.push_back(g);
 155                 g = g->p();
 156         }
 157         std::reverse(this->_path.begin(), this->_path.end());
 158         for (unsigned int i = 1; i < this->_path.size(); i++) {
 159                 this->_path[i]->c(this->cost_build(
 160                         *this->_path[i - 1],
 161                         *this->_path[i]));
 162         }
 163         this->_path.clear();
 164 }
 165
 166 void
 167 RRTS::recompute_path_cc()
 168 {
 169         this->recompute_cc_for_predecessors_and(&this->_goal);
 170 }
 171
 172 double
 173 RRTS::min_gamma_eta() const
 174 {
 175         double ns = this->_nodes.size();
 176         double gamma = pow(log(ns) / ns, 1.0 / 3.0);
 177         return std::min(gamma, this->eta());
 178 }
 179
 180 bool
 181 RRTS::should_continue() const
 182 {
 183         return !this->should_finish();
 184 }
 185
 186 void
 187 RRTS::join_steered(RRTNode* f)
 188 {
 189         while (this->_steered.size() > 0) {
 190                 this->store(this->_steered.front());
 191                 RRTNode* t = &this->_nodes.back();
 192                 t->p(*f);
 193                 t->c(this->cost_build(*f, *t));
 194                 this->_steered.erase(this->_steered.begin());
 195                 f = t;
 196         }
 197 }
 198
 199 bool
 200 RRTS::connect()
 201 {
 202         RRTNode* f = this->_nn;
 203         RRTNode* t = &this->_steered.front();
 204         // Require the steer method to return first node equal to nn:
 205         assert(std::abs(t->x() - f->x()) < 1e-3
 206                 && std::abs(t->y() - f->y()) < 1e-3
 207                 && std::abs(t->h() - f->h()) < 1e-3);
 208         this->_steered.erase(this->_steered.begin());
 209         t = &this->_steered.front();
 210         assert(!(std::abs(t->x() - f->x()) < 1e-3
 211                 && std::abs(t->y() - f->y()) < 1e-3
 212                 && std::abs(t->h() - f->h()) < 1e-3));
 213 #if USE_RRTS
 214         double cost = f->cc() + this->cost_build(*f, *t);
 215         for (auto n: this->nv_) {
 216                 double nc = n->cc() + this->cost_build(*n, *t);
 217                 if (nc < cost) {
 218                         f = n;
 219                         cost = nc;
 220                 }
 221         }
 222         // Check if it's possible to drive from *f to *t. If not, then fallback
 223         // to *f = _nn. This could be also solved by additional steer from *f to
 224         // *t instead of the following code.
 225         this->set_bc_pose_to(*f);
 226         if (!this->_bc.drivable(*t)) {
 227                 f = this->_nn;
 228         }
 229 #endif
 230         this->store(this->_steered.front());
 231         t = &this->_nodes.back();
 232         t->p(*f);
 233         t->c(this->cost_build(*f, *t));
 234         this->_steered.erase(this->_steered.begin());
 235         return true;
 236 }
 237
 238 void
 239 RRTS::rewire()
 240 {
 241         RRTNode *f = &this->_nodes.back();
 242         for (auto n: this->_nv) {
 243                 double fc = f->cc() + this->cost_build(*f, *n);
 244                 this->set_bc_pose_to(*f);
 245                 bool drivable = this->_bc.drivable(*n);
 246                 if (drivable && fc < n->cc()) {
 247                         n->p(*f);
 248                         n->c(this->cost_build(*f, *n));
 249                 }
 250         }
 251 }
 252
 253 bool
 254 RRTS::goal_drivable_from(RRTNode const& f)
 255 {
 256         this->set_bc_pose_to(f);
 257         return this->_bc.drivable(this->_goal);
 258 }
 259
 260 void
 261 RRTS::store(RRTNode n)
 262 {
 263         this->_nodes.push_back(n);
 264 }
 265
 266 double
 267 RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
 268 {
 269         return f.edist(t);
 270 }
 271
 272 double
 273 RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
 274 {
 275         return this->cost_build(f, t);
 276 }
 277
 278 void
 279 RRTS::find_nn(RRTNode const& t)
 280 {
 281         this->_nn = &this->_nodes.front();
 282         this->_cost = this->cost_search(*this->_nn, t);
 283         for (auto& f: this->_nodes) {
 284                 if (this->cost_search(f, t) < this->_cost) {
 285                         this->_nn = &f;
 286                         this->_cost = this->cost_search(f, t);
 287                 }
 288         }
 289 }
 290
 291 void
 292 RRTS::find_nv(RRTNode const& t)
 293 {
 294         this->_nv.clear();
 295         this->_cost = this->min_gamma_eta();
 296         for (auto& f: this->_nodes) {
 297                 if (this->cost_search(f, t) < this->_cost) {
 298                         this->_nv.push_back(&f);
 299                 }
 300         }
 301 }
 302
 303 void
 304 RRTS::compute_path()
 305 {
 306         this->_path.clear();
 307         RRTNode *g = &this->_goal;
 308         if (g->p() == nullptr) {
 309                 return;
 310         }
 311         while (g != nullptr && this->_path.size() < 10000) {
 312                 /* FIXME in ext13
 313                  *
 314                  * There shouldn't be this->_path.size() < 10000 condition.
 315                  * However, the RRTS::compute_path() called from
 316                  * RRTExt13::compute_path tends to re-allocate this->_path
 317                  * infinitely. There's probably node->p() = &node somewhere...
 318                  */
 319                 this->_path.push_back(g);
 320                 g = g->p();
 321         }
 322         std::reverse(this->_path.begin(), this->_path.end());
 323 }
 324
 325 RRTS::RRTS() : _goal(0.0, 0.0, 0.0, 0.0), _gen(std::random_device{}())
 326 {
 327         this->_nodes.reserve(4000000);
 328         this->_steered.reserve(1000);
 329         this->_path.reserve(10000);
 330         this->_nv.reserve(1000);
 331         this->store(RRTNode()); // root
 332 }
 333
 334 void
 335 RRTS::set_bc_pose_to(Pose const& p)
 336 {
 337         this->_bc.set_pose_to(p);
 338 }
 339
 340 void
 341 RRTS::set_bc_to_become(std::string what)
 342 {
 343         this->_bc.become(what);
 344 }
 345
 346 RRTGoal const&
 347 RRTS::goal(void) const
 348 {
 349         return this->_goal;
 350 }
 351
 352 void
 353 RRTS::goal(double x, double y, double b, double e)
 354 {
 355         this->_goal = RRTGoal(x, y, b, e);
 356 }
 357
 358 unsigned int
 359 RRTS::icnt(void) const
 360 {
 361         return this->_icnt;
 362 }
 363
 364 void
 365 RRTS::icnt(unsigned int i)
 366 {
 367         this->_icnt = i;
 368 }
 369
 370 unsigned int
 371 RRTS::icnt_max(void) const
 372 {
 373         return this->_icnt_max;
 374 }
 375
 376 void
 377 RRTS::icnt_max(unsigned int i)
 378 {
 379         this->_icnt_max = i;
 380 }
 381
 382 void
 383 RRTS::tstart(void)
 384 {
 385         this->_ter.start();
 386 }
 387
 388 double
 389 RRTS::scnt() const
 390 {
 391         return this->_ter.scnt();
 392 }
 393
 394 void
 395 RRTS::set_init_pose_to(Pose const& p)
 396 {
 397         this->_nodes.front().x(p.x());
 398         this->_nodes.front().y(p.y());
 399         this->_nodes.front().h(p.h());
 400 }
 401
 402 std::vector<Pose>
 403 RRTS::path() const
 404 {
 405         std::vector<Pose> path;
 406         for (auto n: this->_path) {
 407                 path.push_back(Pose(n->x(), n->y(), n->h()));
 408         }
 409         return path;
 410 }
 411
 412 double
 413 RRTS::path_cost() const
 414 {
 415         return this->_goal.cc();
 416 }
 417
 418 double
 419 RRTS::last_path_cost(void) const
 420 {
 421         if (this->_logged_paths.size() == 0) {
 422                 return 0.0;
 423         }
 424         assert(this->_logged_paths.back().size() > 0);
 425         return this->_logged_paths.back().back().cc();
 426 }
 427
 428 double
 429 RRTS::eta() const
 430 {
 431         return this->_eta;
 432 }
 433
 434 void
 435 RRTS::eta(double e)
 436 {
 437         this->_eta = e;
 438 }
 439
 440 Json::Value
 441 RRTS::json(void) const
 442 {
 443         Json::Value jvo;
 444         unsigned int i = 0, j = 0;
 445         for (auto path: this->_logged_paths) {
 446                 i = 0;
 447                 for (auto n: path) {
 448                         jvo["paths"][j][i][0] = n.x();
 449                         jvo["paths"][j][i][1] = n.y();
 450                         jvo["paths"][j][i][2] = n.h();
 451                         jvo["paths"][j][i][3] = n.sp();
 452                         jvo["paths"][j][i][4] = n.st();
 453                         jvo["paths"][j][i][5] = false;
 454                         if (n.p_is_cusp()) {
 455                                 assert(i > 0);
 456                                 jvo["paths"][j][i - 1][5] = true;
 457                         }
 458                         i++;
 459                 }
 460                 // Initial point is part of the first segment.
 461                 jvo["paths"][j][0][3] = jvo["paths"][j][1][3];
 462                 jvo["paths"][j][0][4] = jvo["paths"][j][1][4];
 463                 // Goal point is part of the last segment.
 464                 jvo["paths"][j][i - 1][3] = jvo["paths"][j][i - 2][3];
 465                 jvo["paths"][j][i - 1][4] = jvo["paths"][j][i - 2][4];
 466                 // --
 467                 jvo["costs"][j] = path.back().cc();
 468                 j++;
 469         }
 470         i = 0;
 471         for (auto n: this->_path) {
 472                 jvo["paths"][j][i][0] = n->x();
 473                 jvo["paths"][j][i][1] = n->y();
 474                 jvo["paths"][j][i][2] = n->h();
 475                 jvo["paths"][j][i][3] = n->sp();
 476                 jvo["paths"][j][i][4] = n->st();
 477                 jvo["paths"][j][i][5] = n->p_is_cusp();
 478                 jvo["path"][i][0] = n->x();
 479                 jvo["path"][i][1] = n->y();
 480                 jvo["path"][i][2] = n->h();
 481                 jvo["path"][i][3] = n->sp();
 482                 jvo["path"][i][4] = n->st();
 483                 jvo["path"][i][5] = false;
 484                 if (n->p_is_cusp()) {
 485                         assert(i > 0);
 486                         jvo["path"][i - 1][5] = true;
 487                 }
 488                 i++;
 489         }
 490         // Initial point is part of the first segment.
 491         jvo["path"][0][3] = jvo["path"][1][3];
 492         jvo["path"][0][4] = jvo["path"][1][4];
 493         // Goal point is part of the last segment.
 494         jvo["path"][i - 1][3] = jvo["path"][i - 2][3];
 495         jvo["path"][i - 1][4] = jvo["path"][i - 2][4];
 496         // --
 497         jvo["costs"][j] = this->_path.back()->cc();
 498         jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
 499         jvo["cost"] = this->path_cost();
 500         jvo["time"] = this->scnt();
 501         return jvo;
 502 }
 503
 504 void
 505 RRTS::json(Json::Value jvi)
 506 {
 507         assert(jvi["init"] != Json::nullValue);
 508         assert(jvi["goal"] != Json::nullValue);
 509         this->set_init_pose_to(Pose(
 510                 jvi["init"][0].asDouble(),
 511                 jvi["init"][1].asDouble(),
 512                 jvi["init"][2].asDouble()));
 513         if (jvi["goal"].size() == 4) {
 514                 this->goal(
 515                         jvi["goal"][0].asDouble(),
 516                         jvi["goal"][1].asDouble(),
 517                         jvi["goal"][2].asDouble(),
 518                         jvi["goal"][3].asDouble());
 519         } else {
 520                 this->goal(
 521                         jvi["goal"][0].asDouble(),
 522                         jvi["goal"][1].asDouble(),
 523                         jvi["goal"][2].asDouble(),
 524                         jvi["goal"][2].asDouble());
 525         }
 526 }
 527
 528 bool
 529 RRTS::next()
 530 {
 531         if (this->icnt() == 0) {
 532                 this->tstart();
 533         }
 534         auto rs = this->sample();
 535 #if 1 // anytime RRTs
 536 {
 537         double d1 = this->cost_search(this->_nodes.front(), rs);
 538         double d2 = this->cost_search(rs, this->_goal);
 539         if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
 540                 auto& last_path = this->_logged_paths.back();
 541                 rs = last_path[rand() % last_path.size()];
 542         }
 543 }
 544 #endif
 545         this->find_nn(rs);
 546         this->steer(*this->_nn, rs);
 547         if (this->collide_steered()) {
 548                 return this->should_continue();
 549         }
 550 #if USE_RRTS
 551         this->find_nv(this->_steered.front());
 552 #endif
 553         if (!this->connect()) {
 554                 return this->should_continue();
 555         }
 556 #if USE_RRTS
 557         this->rewire();
 558 #endif
 559         unsigned int ss = this->_steered.size();
 560         this->join_steered(&this->_nodes.back());
 561         RRTNode* just_added = &this->_nodes.back();
 562         bool gf = false;
 563         while (ss > 0 && just_added != nullptr) {
 564                 this->steer(*just_added, this->_goal);
 565                 if (this->collide_steered()) {
 566                         ss--;
 567                         just_added = just_added->p();
 568                         continue;
 569                 }
 570                 // The first of steered is the same as just_added.
 571                 this->_steered.erase(this->_steered.begin());
 572                 this->join_steered(just_added);
 573                 bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
 574                 bool gd = this->goal_drivable_from(this->_nodes.back());
 575                 if (gn && gd) {
 576                         double nc = this->cost_build(
 577                                 this->_nodes.back(),
 578                                 this->_goal);
 579                         double ncc = this->_nodes.back().cc() + nc;
 580                         if (this->_goal.p() == nullptr
 581                                         || ncc < this->_goal.cc()) {
 582                                 this->_goal.p(this->_nodes.back(), true);
 583                                 this->_goal.c(nc);
 584                                 gf = true;
 585                         }
 586                 }
 587                 ss--;
 588                 just_added = just_added->p();
 589         }
 590         if (gf) {
 591                 this->compute_path();
 592         }
 593         this->_time = this->scnt();
 594         this->icnt(this->icnt() + 1);
 595         return this->should_continue();
 596 }
 597
 598 void
 599 RRTS::reset()
 600 {
 601         if (this->path_cost() != 0.0
 602                         && this->path_cost() < this->last_path_cost()) {
 603                 this->_logged_paths.push_back(std::vector<RRTNode>());
 604                 auto& last_path = this->_logged_paths.back();
 605                 last_path.reserve(this->_path.size());
 606                 RRTNode* p = nullptr;
 607                 for (auto n: this->_path) {
 608                         last_path.push_back(*n);
 609                         if (p != nullptr) {
 610                                 last_path.back().p(*p);
 611                         }
 612                         p = &last_path.back();
 613                 }
 614                 // Test that last path cost matches.
 615                 auto last_path_cost = last_path.back().cc();
 616                 for (unsigned int i = 1; i < last_path.size(); i++) {
 617                         last_path[i].c(this->cost_build(
 618                                 last_path[i - 1],
 619                                 last_path[i]));
 620                 }
 621                 assert(last_path_cost == last_path.back().cc());
 622         }
 623         this->_goal = RRTGoal(
 624                 this->_goal.x(),
 625                 this->_goal.y(),
 626                 this->_goal.b(),
 627                 this->_goal.e());
 628         this->_path.clear();
 629         this->_steered.clear();
 630         this->_nodes.erase(this->_nodes.begin() + 1, this->_nodes.end());
 631         this->_nv.clear();
 632         this->_nn = nullptr;
 633         this->_bc = BicycleCar();
 634 }
 635
 636 } // namespace rrts