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