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