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() const
  67 {
  68         return this->_cusp;
  69 }
  70
  71 void
  72 RRTNode::cusp(RRTNode const& p)
  73 {
  74         this->_cusp = p.cusp();
  75         if (this->sp() != p.sp() || this->sp() == 0.0) {
  76                 this->_cusp++;
  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(*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 #if USE_RRTS
 155         double cost = f->cc() + this->cost_build(*f, *t);
 156         for (auto n: this->nv_) {
 157                 double nc = n->cc() + this->cost_build(*n, *t);
 158                 if (nc < cost) {
 159                         f = n;
 160                         cost = nc;
 161                 }
 162         }
 163         // Check if it's possible to drive from *f to *t. If not, then fallback
 164         // to *f = _nn. This could be also solved by additional steer from *f to
 165         // *t instead of the following code.
 166         this->set_bc_pose_to(*f);
 167         if (!this->_bc.drivable(*t)) {
 168                 f = this->_nn;
 169         }
 170 #endif
 171         this->store(this->_steered.front());
 172         t = &this->_nodes.back();
 173         t->p(*f);
 174         t->c(this->cost_build(*f, *t));
 175         t->cusp(*f);
 176         this->_steered.erase(this->_steered.begin());
 177         return true;
 178 }
 179
 180 void
 181 RRTS::rewire()
 182 {
 183         RRTNode *f = &this->_nodes.back();
 184         for (auto n: this->_nv) {
 185                 double fc = f->cc() + this->cost_build(*f, *n);
 186                 this->set_bc_pose_to(*f);
 187                 bool drivable = this->_bc.drivable(*n);
 188                 if (drivable && fc < n->cc()) {
 189                         n->p(*f);
 190                         n->c(this->cost_build(*f, *n));
 191                 }
 192         }
 193 }
 194
 195 bool
 196 RRTS::goal_drivable_from(RRTNode const& f)
 197 {
 198         this->set_bc_pose_to(f);
 199         return this->_bc.drivable(this->_goal);
 200 }
 201
 202 void
 203 RRTS::store(RRTNode n)
 204 {
 205         this->_nodes.push_back(n);
 206 }
 207
 208 double
 209 RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
 210 {
 211         return f.edist(t);
 212 }
 213
 214 double
 215 RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
 216 {
 217         return this->cost_build(f, t);
 218 }
 219
 220 void
 221 RRTS::find_nn(RRTNode const& t)
 222 {
 223         this->_nn = &this->_nodes.front();
 224         this->_cost = this->cost_search(*this->_nn, t);
 225         for (auto& f: this->_nodes) {
 226                 if (this->cost_search(f, t) < this->_cost) {
 227                         this->_nn = &f;
 228                         this->_cost = this->cost_search(f, t);
 229                 }
 230         }
 231 }
 232
 233 void
 234 RRTS::find_nv(RRTNode const& t)
 235 {
 236         this->_nv.clear();
 237         this->_cost = this->min_gamma_eta();
 238         for (auto& f: this->_nodes) {
 239                 if (this->cost_search(f, t) < this->_cost) {
 240                         this->_nv.push_back(&f);
 241                 }
 242         }
 243 }
 244
 245 void
 246 RRTS::compute_path()
 247 {
 248         this->_path.clear();
 249         RRTNode *g = &this->_goal;
 250         if (g->p() == nullptr) {
 251                 return;
 252         }
 253         while (g != nullptr && this->_path.size() < 10000) {
 254                 /* FIXME in ext13
 255                  *
 256                  * There shouldn't be this->_path.size() < 10000 condition.
 257                  * However, the RRTS::compute_path() called from
 258                  * RRTExt13::compute_path tends to re-allocate this->_path
 259                  * infinitely. There's probably node->p() = &node somewhere...
 260                  */
 261                 this->_path.push_back(g);
 262                 g = g->p();
 263         }
 264         std::reverse(this->_path.begin(), this->_path.end());
 265 }
 266
 267 RRTS::RRTS() : _goal(0.0, 0.0, 0.0, 0.0), _gen(std::random_device{}())
 268 {
 269         this->_nodes.reserve(4000000);
 270         this->_steered.reserve(1000);
 271         this->_path.reserve(10000);
 272         this->_nv.reserve(1000);
 273         this->store(RRTNode()); // root
 274 }
 275
 276 void
 277 RRTS::set_bc_pose_to(Pose const& p)
 278 {
 279         this->_bc.set_pose_to(p);
 280 }
 281
 282 RRTGoal const&
 283 RRTS::goal(void) const
 284 {
 285         return this->_goal;
 286 }
 287
 288 void
 289 RRTS::goal(double x, double y, double b, double e)
 290 {
 291         this->_goal = RRTGoal(x, y, b, e);
 292 }
 293
 294 unsigned int
 295 RRTS::icnt(void) const
 296 {
 297         return this->_icnt;
 298 }
 299
 300 void
 301 RRTS::icnt(unsigned int i)
 302 {
 303         this->_icnt = i;
 304 }
 305
 306 unsigned int
 307 RRTS::icnt_max(void) const
 308 {
 309         return this->_icnt_max;
 310 }
 311
 312 void
 313 RRTS::icnt_max(unsigned int i)
 314 {
 315         this->_icnt_max = i;
 316 }
 317
 318 void
 319 RRTS::tstart(void)
 320 {
 321         this->_ter.start();
 322 }
 323
 324 double
 325 RRTS::scnt() const
 326 {
 327         return this->_ter.scnt();
 328 }
 329
 330 void
 331 RRTS::start(double x, double y, double h)
 332 {
 333         this->_nodes.front().x(x);
 334         this->_nodes.front().y(y);
 335         this->_nodes.front().h(h);
 336 }
 337
 338 std::vector<Pose>
 339 RRTS::path() const
 340 {
 341         std::vector<Pose> path;
 342         for (auto n: this->_path) {
 343                 path.push_back(Pose(n->x(), n->y(), n->h()));
 344         }
 345         return path;
 346 }
 347
 348 double
 349 RRTS::path_cost() const
 350 {
 351         return this->_goal.cc();
 352 }
 353
 354 double
 355 RRTS::last_path_cost(void) const
 356 {
 357         if (this->_last_path.size() == 0) {
 358                 return 999.9;
 359         } else {
 360                 return this->_last_path.back().cc();
 361         }
 362 }
 363
 364 double
 365 RRTS::eta() const
 366 {
 367         return this->_eta;
 368 }
 369
 370 void
 371 RRTS::eta(double e)
 372 {
 373         this->_eta = e;
 374 }
 375
 376 Json::Value
 377 RRTS::json() const
 378 {
 379         Json::Value jvo;
 380         unsigned int i = 0;
 381         for (auto n: this->_path) { // TODO because path() has just x, y, h
 382                 jvo["path"][i][0] = n->x();
 383                 jvo["path"][i][1] = n->y();
 384                 jvo["path"][i][2] = n->h();
 385                 jvo["path"][i][3] = n->sp();
 386                 jvo["path"][i][4] = n->st();
 387                 i++;
 388         }
 389         jvo["goal_cc"] = this->_goal.cc();
 390         jvo["time"] = this->scnt();
 391         return jvo;
 392 }
 393
 394 void
 395 RRTS::json(Json::Value jvi)
 396 {
 397         assert(jvi["init"] != Json::nullValue);
 398         assert(jvi["goal"] != Json::nullValue);
 399         this->start(
 400                 jvi["init"][0].asDouble(),
 401                 jvi["init"][1].asDouble(),
 402                 jvi["init"][2].asDouble());
 403         if (jvi["goal"].size() == 4) {
 404                 this->goal(
 405                         jvi["goal"][0].asDouble(),
 406                         jvi["goal"][1].asDouble(),
 407                         jvi["goal"][2].asDouble(),
 408                         jvi["goal"][3].asDouble());
 409         } else {
 410                 this->goal(
 411                         jvi["goal"][0].asDouble(),
 412                         jvi["goal"][1].asDouble(),
 413                         jvi["goal"][2].asDouble(),
 414                         jvi["goal"][2].asDouble());
 415         }
 416 }
 417
 418 bool
 419 RRTS::next()
 420 {
 421         if (this->icnt() == 0) {
 422                 this->tstart();
 423         }
 424         auto rs = this->sample();
 425 #if 1 // anytime RRTs
 426 {
 427         double d1 = this->cost_search(this->_nodes.front(), rs);
 428         double d2 = this->cost_search(rs, this->_goal);
 429         if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
 430                 rs = this->_last_path[rand() % this->_last_path.size()];
 431         }
 432 }
 433 #endif
 434         this->find_nn(rs);
 435         this->steer(*this->_nn, rs);
 436         if (this->collide_steered()) {
 437                 return this->should_continue();
 438         }
 439 #if USE_RRTS
 440         this->find_nv(this->_steered.front());
 441 #endif
 442         if (!this->connect()) {
 443                 return this->should_continue();
 444         }
 445 #if USE_RRTS
 446         this->rewire();
 447 #endif
 448         unsigned int ss = this->_steered.size();
 449         this->join_steered(&this->_nodes.back());
 450         RRTNode* just_added = &this->_nodes.back();
 451         bool gf = false;
 452         while (ss > 0 && just_added->p() != nullptr) {
 453                 this->steer(*just_added, this->_goal);
 454                 if (this->collide_steered()) {
 455                         ss--;
 456                         just_added = just_added->p();
 457                         continue;
 458                 }
 459                 this->join_steered(just_added);
 460                 bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
 461                 bool gd = this->goal_drivable_from(this->_nodes.back());
 462                 if (gn && gd) {
 463                         double nc = this->cost_build(
 464                                 this->_nodes.back(),
 465                                 this->_goal);
 466                         double ncc = this->_nodes.back().cc() + nc;
 467                         if (this->_goal.p() == nullptr
 468                                         || ncc < this->_goal.cc()) {
 469                                 this->_goal.p(this->_nodes.back());
 470                                 this->_goal.c(nc);
 471                                 gf = true;
 472                         }
 473                 }
 474                 ss--;
 475                 just_added = just_added->p();
 476         }
 477         if (gf) {
 478                 this->compute_path();
 479         }
 480         this->_time = this->scnt();
 481         this->icnt(this->icnt() + 1);
 482         return this->should_continue();
 483 }
 484
 485 void
 486 RRTS::reset()
 487 {
 488         if (this->path_cost() != 0.0
 489                         && this->path_cost() < this->last_path_cost()) {
 490                 this->_last_path.clear();
 491                 for (auto n: this->_path) {
 492                         this->_last_path.push_back(*n);
 493                         // FIXME _last_path nodes' pointers to parents
 494                 }
 495         }
 496         this->_goal = RRTGoal(
 497                 this->_goal.x(),
 498                 this->_goal.y(),
 499                 this->_goal.b(),
 500                 this->_goal.e());
 501         this->_path.clear();
 502         this->_steered.clear();
 503         this->_nodes.erase(this->_nodes.begin() + 1, this->_nodes.end());
 504         this->_nv.clear();
 505         this->_nn = nullptr;
 506         this->_bc = BicycleCar();
 507 }
 508
 509 } // namespace rrts