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 (p.sp() == 0) {
 120                 assert(this->sp() != 0);
 121                 if (p.p()) {
 122                         if (sgn(p.p()->sp()) != sgn(this->sp())) {
 123                                 return true;
 124                         } else {
 125                                 return false;
 126                         }
 127                 } else {
 128                         return true; // only root has no parent and it is cusp
 129                 }
 130         } else {
 131                 if (this->sp() == 0) {
 132                         return false; // this is cusp, not the parent
 133                 } else if (sgn(p.sp()) != sgn(this->sp())) {
 134                         return true;
 135                 } else {
 136                         return false;
 137                 }
 138         }
 139 }
 140
 141 bool
 142 RRTNode::operator==(RRTNode const& n)
 143 {
 144         return this == &n;
 145 }
 146
 147 void
 148 RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
 149 {
 150         assert(this->_path.size() == 0);
 151         while (g != nullptr) {
 152                 this->_path.push_back(g);
 153                 g = g->p();
 154         }
 155         std::reverse(this->_path.begin(), this->_path.end());
 156         for (unsigned int i = 1; i < this->_path.size(); i++) {
 157                 this->_path[i]->c(this->cost_build(
 158                         *this->_path[i - 1],
 159                         *this->_path[i]));
 160         }
 161         this->_path.clear();
 162 }
 163
 164 void
 165 RRTS::recompute_path_cc()
 166 {
 167         this->recompute_cc_for_predecessors_and(&this->_goal);
 168 }
 169
 170 double
 171 RRTS::min_gamma_eta() const
 172 {
 173         double ns = this->_nodes.size();
 174         double gamma = pow(log(ns) / ns, 1.0 / 3.0);
 175         return std::min(gamma, this->eta());
 176 }
 177
 178 bool
 179 RRTS::should_continue() const
 180 {
 181         return !this->should_finish();
 182 }
 183
 184 void
 185 RRTS::join_steered(RRTNode* f)
 186 {
 187         while (this->_steered.size() > 0) {
 188                 this->store(this->_steered.front());
 189                 RRTNode* t = &this->_nodes.back();
 190                 t->p(*f);
 191                 t->c(this->cost_build(*f, *t));
 192                 this->_steered.erase(this->_steered.begin());
 193                 f = t;
 194         }
 195 }
 196
 197 bool
 198 RRTS::connect()
 199 {
 200         RRTNode* f = this->_nn;
 201         RRTNode* t = &this->_steered.front();
 202         // Require the steer method to return first node equal to nn:
 203         assert(std::abs(t->x() - f->x()) < 1e-3);
 204         assert(std::abs(t->y() - f->x()) < 1e-3);
 205         assert(std::abs(t->h() - f->x()) < 1e-3);
 206         this->_steered.erase(this->_steered.begin());
 207         t = &this->_steered.front();
 208 #if USE_RRTS
 209         double cost = f->cc() + this->cost_build(*f, *t);
 210         for (auto n: this->nv_) {
 211                 double nc = n->cc() + this->cost_build(*n, *t);
 212                 if (nc < cost) {
 213                         f = n;
 214                         cost = nc;
 215                 }
 216         }
 217         // Check if it's possible to drive from *f to *t. If not, then fallback
 218         // to *f = _nn. This could be also solved by additional steer from *f to
 219         // *t instead of the following code.
 220         this->set_bc_pose_to(*f);
 221         if (!this->_bc.drivable(*t)) {
 222                 f = this->_nn;
 223         }
 224 #endif
 225         this->store(this->_steered.front());
 226         t = &this->_nodes.back();
 227         t->p(*f);
 228         t->c(this->cost_build(*f, *t));
 229         this->_steered.erase(this->_steered.begin());
 230         return true;
 231 }
 232
 233 void
 234 RRTS::rewire()
 235 {
 236         RRTNode *f = &this->_nodes.back();
 237         for (auto n: this->_nv) {
 238                 double fc = f->cc() + this->cost_build(*f, *n);
 239                 this->set_bc_pose_to(*f);
 240                 bool drivable = this->_bc.drivable(*n);
 241                 if (drivable && fc < n->cc()) {
 242                         n->p(*f);
 243                         n->c(this->cost_build(*f, *n));
 244                 }
 245         }
 246 }
 247
 248 bool
 249 RRTS::goal_drivable_from(RRTNode const& f)
 250 {
 251         this->set_bc_pose_to(f);
 252         return this->_bc.drivable(this->_goal);
 253 }
 254
 255 void
 256 RRTS::store(RRTNode n)
 257 {
 258         this->_nodes.push_back(n);
 259 }
 260
 261 double
 262 RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
 263 {
 264         return f.edist(t);
 265 }
 266
 267 double
 268 RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
 269 {
 270         return this->cost_build(f, t);
 271 }
 272
 273 void
 274 RRTS::find_nn(RRTNode const& t)
 275 {
 276         this->_nn = &this->_nodes.front();
 277         this->_cost = this->cost_search(*this->_nn, t);
 278         for (auto& f: this->_nodes) {
 279                 if (this->cost_search(f, t) < this->_cost) {
 280                         this->_nn = &f;
 281                         this->_cost = this->cost_search(f, t);
 282                 }
 283         }
 284 }
 285
 286 void
 287 RRTS::find_nv(RRTNode const& t)
 288 {
 289         this->_nv.clear();
 290         this->_cost = this->min_gamma_eta();
 291         for (auto& f: this->_nodes) {
 292                 if (this->cost_search(f, t) < this->_cost) {
 293                         this->_nv.push_back(&f);
 294                 }
 295         }
 296 }
 297
 298 void
 299 RRTS::compute_path()
 300 {
 301         this->_path.clear();
 302         RRTNode *g = &this->_goal;
 303         if (g->p() == nullptr) {
 304                 return;
 305         }
 306         while (g != nullptr && this->_path.size() < 10000) {
 307                 /* FIXME in ext13
 308                  *
 309                  * There shouldn't be this->_path.size() < 10000 condition.
 310                  * However, the RRTS::compute_path() called from
 311                  * RRTExt13::compute_path tends to re-allocate this->_path
 312                  * infinitely. There's probably node->p() = &node somewhere...
 313                  */
 314                 this->_path.push_back(g);
 315                 g = g->p();
 316         }
 317         std::reverse(this->_path.begin(), this->_path.end());
 318 }
 319
 320 RRTS::RRTS() : _goal(0.0, 0.0, 0.0, 0.0), _gen(std::random_device{}())
 321 {
 322         this->_nodes.reserve(4000000);
 323         this->_steered.reserve(1000);
 324         this->_path.reserve(10000);
 325         this->_nv.reserve(1000);
 326         this->store(RRTNode()); // root
 327 }
 328
 329 void
 330 RRTS::set_bc_pose_to(Pose const& p)
 331 {
 332         this->_bc.set_pose_to(p);
 333 }
 334
 335 void
 336 RRTS::set_bc_to_become(std::string what)
 337 {
 338         this->_bc.become(what);
 339 }
 340
 341 RRTGoal const&
 342 RRTS::goal(void) const
 343 {
 344         return this->_goal;
 345 }
 346
 347 void
 348 RRTS::goal(double x, double y, double b, double e)
 349 {
 350         this->_goal = RRTGoal(x, y, b, e);
 351 }
 352
 353 unsigned int
 354 RRTS::icnt(void) const
 355 {
 356         return this->_icnt;
 357 }
 358
 359 void
 360 RRTS::icnt(unsigned int i)
 361 {
 362         this->_icnt = i;
 363 }
 364
 365 unsigned int
 366 RRTS::icnt_max(void) const
 367 {
 368         return this->_icnt_max;
 369 }
 370
 371 void
 372 RRTS::icnt_max(unsigned int i)
 373 {
 374         this->_icnt_max = i;
 375 }
 376
 377 void
 378 RRTS::tstart(void)
 379 {
 380         this->_ter.start();
 381 }
 382
 383 double
 384 RRTS::scnt() const
 385 {
 386         return this->_ter.scnt();
 387 }
 388
 389 void
 390 RRTS::set_init_pose_to(Pose const& p)
 391 {
 392         this->_nodes.front().x(p.x());
 393         this->_nodes.front().y(p.y());
 394         this->_nodes.front().h(p.h());
 395 }
 396
 397 std::vector<Pose>
 398 RRTS::path() const
 399 {
 400         std::vector<Pose> path;
 401         for (auto n: this->_path) {
 402                 path.push_back(Pose(n->x(), n->y(), n->h()));
 403         }
 404         return path;
 405 }
 406
 407 double
 408 RRTS::path_cost() const
 409 {
 410         return this->_goal.cc();
 411 }
 412
 413 double
 414 RRTS::last_path_cost(void) const
 415 {
 416         if (this->_logged_paths.size() == 0) {
 417                 return 0.0;
 418         }
 419         assert(this->_logged_paths.back().size() > 0);
 420         return this->_logged_paths.back().back().cc();
 421 }
 422
 423 double
 424 RRTS::eta() const
 425 {
 426         return this->_eta;
 427 }
 428
 429 void
 430 RRTS::eta(double e)
 431 {
 432         this->_eta = e;
 433 }
 434
 435 Json::Value
 436 RRTS::json(void) const
 437 {
 438         Json::Value jvo;
 439         unsigned int i = 0, j = 0;
 440         for (auto path: this->_logged_paths) {
 441                 i = 0;
 442                 for (auto n: path) {
 443                         jvo["paths"][j][i][0] = n.x();
 444                         jvo["paths"][j][i][1] = n.y();
 445                         jvo["paths"][j][i][2] = n.h();
 446                         jvo["paths"][j][i][3] = n.sp();
 447                         jvo["paths"][j][i][4] = n.st();
 448                         jvo["paths"][j][i][5] = n.p_is_cusp();
 449                         i++;
 450                 }
 451                 jvo["costs"][j] = path.back().cc();
 452                 j++;
 453         }
 454         i = 0;
 455         for (auto n: this->_path) {
 456                 jvo["paths"][j][i][0] = n->x();
 457                 jvo["paths"][j][i][1] = n->y();
 458                 jvo["paths"][j][i][2] = n->h();
 459                 jvo["paths"][j][i][3] = n->sp();
 460                 jvo["paths"][j][i][4] = n->st();
 461                 jvo["paths"][j][i][5] = n->p_is_cusp();
 462                 jvo["path"][i][0] = n->x();
 463                 jvo["path"][i][1] = n->y();
 464                 jvo["path"][i][2] = n->h();
 465                 jvo["path"][i][3] = n->sp();
 466                 jvo["path"][i][4] = n->st();
 467                 jvo["path"][i][5] = n->p_is_cusp();
 468                 i++;
 469         }
 470         jvo["costs"][j] = this->_path.back()->cc();
 471         j++;
 472         jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
 473         jvo["cost"] = this->path_cost();
 474         jvo["time"] = this->scnt();
 475         return jvo;
 476 }
 477
 478 void
 479 RRTS::json(Json::Value jvi)
 480 {
 481         assert(jvi["init"] != Json::nullValue);
 482         assert(jvi["goal"] != Json::nullValue);
 483         this->set_init_pose_to(Pose(
 484                 jvi["init"][0].asDouble(),
 485                 jvi["init"][1].asDouble(),
 486                 jvi["init"][2].asDouble()));
 487         if (jvi["goal"].size() == 4) {
 488                 this->goal(
 489                         jvi["goal"][0].asDouble(),
 490                         jvi["goal"][1].asDouble(),
 491                         jvi["goal"][2].asDouble(),
 492                         jvi["goal"][3].asDouble());
 493         } else {
 494                 this->goal(
 495                         jvi["goal"][0].asDouble(),
 496                         jvi["goal"][1].asDouble(),
 497                         jvi["goal"][2].asDouble(),
 498                         jvi["goal"][2].asDouble());
 499         }
 500 }
 501
 502 bool
 503 RRTS::next()
 504 {
 505         if (this->icnt() == 0) {
 506                 this->tstart();
 507         }
 508         auto rs = this->sample();
 509 #if 1 // anytime RRTs
 510 {
 511         double d1 = this->cost_search(this->_nodes.front(), rs);
 512         double d2 = this->cost_search(rs, this->_goal);
 513         if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
 514                 auto& last_path = this->_logged_paths.back();
 515                 rs = last_path[rand() % last_path.size()];
 516         }
 517 }
 518 #endif
 519         this->find_nn(rs);
 520         this->steer(*this->_nn, rs);
 521         if (this->collide_steered()) {
 522                 return this->should_continue();
 523         }
 524 #if USE_RRTS
 525         this->find_nv(this->_steered.front());
 526 #endif
 527         if (!this->connect()) {
 528                 return this->should_continue();
 529         }
 530 #if USE_RRTS
 531         this->rewire();
 532 #endif
 533         unsigned int ss = this->_steered.size();
 534         this->join_steered(&this->_nodes.back());
 535         RRTNode* just_added = &this->_nodes.back();
 536         bool gf = false;
 537         while (ss > 0 && just_added != nullptr) {
 538                 this->steer(*just_added, this->_goal);
 539                 if (this->collide_steered()) {
 540                         ss--;
 541                         just_added = just_added->p();
 542                         continue;
 543                 }
 544                 // The first of steered is the same as just_added.
 545                 this->_steered.erase(this->_steered.begin());
 546                 this->join_steered(just_added);
 547                 bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
 548                 bool gd = this->goal_drivable_from(this->_nodes.back());
 549                 if (gn && gd) {
 550                         double nc = this->cost_build(
 551                                 this->_nodes.back(),
 552                                 this->_goal);
 553                         double ncc = this->_nodes.back().cc() + nc;
 554                         if (this->_goal.p() == nullptr
 555                                         || ncc < this->_goal.cc()) {
 556                                 this->_goal.p(this->_nodes.back(), true);
 557                                 this->_goal.c(nc);
 558                                 gf = true;
 559                         }
 560                 }
 561                 ss--;
 562                 just_added = just_added->p();
 563         }
 564         if (gf) {
 565                 this->compute_path();
 566         }
 567         this->_time = this->scnt();
 568         this->icnt(this->icnt() + 1);
 569         return this->should_continue();
 570 }
 571
 572 void
 573 RRTS::reset()
 574 {
 575         if (this->path_cost() != 0.0
 576                         && this->path_cost() < this->last_path_cost()) {
 577                 this->_logged_paths.push_back(std::vector<RRTNode>());
 578                 auto& last_path = this->_logged_paths.back();
 579                 last_path.reserve(this->_path.size());
 580                 RRTNode* p = nullptr;
 581                 for (auto n: this->_path) {
 582                         last_path.push_back(*n);
 583                         if (p != nullptr) {
 584                                 last_path.back().p(*p);
 585                         }
 586                         p = &last_path.back();
 587                 }
 588                 // Test that last path cost matches.
 589                 auto last_path_cost = last_path.back().cc();
 590                 for (unsigned int i = 1; i < last_path.size(); i++) {
 591                         last_path[i].c(this->cost_build(
 592                                 last_path[i - 1],
 593                                 last_path[i]));
 594                 }
 595                 assert(last_path_cost == last_path.back().cc());
 596         }
 597         this->_goal = RRTGoal(
 598                 this->_goal.x(),
 599                 this->_goal.y(),
 600                 this->_goal.b(),
 601                 this->_goal.e());
 602         this->_path.clear();
 603         this->_steered.clear();
 604         this->_nodes.erase(this->_nodes.begin() + 1, this->_nodes.end());
 605         this->_nv.clear();
 606         this->_nn = nullptr;
 607         this->_bc = BicycleCar();
 608 }
 609
 610 } // namespace rrts