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 namespace rrts {
  12
  13 void
  14 Ter::start()
  15 {
  16         this->tstart_ = std::chrono::high_resolution_clock::now();
  17 }
  18
  19 double
  20 Ter::scnt() const
  21 {
  22         using namespace std::chrono;
  23         auto t = high_resolution_clock::now() - this->tstart_;
  24         auto d = duration_cast<duration<double>>(t);
  25         return d.count();
  26 }
  27
  28 double
  29 RRTNode::c() const
  30 {
  31         return this->c_;
  32 }
  33
  34 void
  35 RRTNode::c(double c)
  36 {
  37         assert(this->p_ != nullptr);
  38         this->c_ = c;
  39         this->cc_ = this->p_->cc() + c;
  40 }
  41
  42 double
  43 RRTNode::cc() const
  44 {
  45         return this->cc_;
  46 }
  47
  48 RRTNode*
  49 RRTNode::p() const
  50 {
  51         return this->p_;
  52 }
  53
  54 void
  55 RRTNode::p(RRTNode& p)
  56 {
  57         if (this != &p) {
  58                 this->p_ = &p;
  59         }
  60 }
  61
  62 unsigned int
  63 RRTNode::cusp() const
  64 {
  65         return this->cusp_;
  66 }
  67
  68 void
  69 RRTNode::cusp(RRTNode const& p)
  70 {
  71         this->cusp_ = p.cusp();
  72         if (this->sp() != p.sp() || this->sp() == 0.0) {
  73                 this->cusp_++;
  74         }
  75 }
  76
  77 bool
  78 RRTNode::operator==(RRTNode const& n)
  79 {
  80         return this == &n;
  81 }
  82
  83 void
  84 RRTS::recompute_cc(RRTNode* g)
  85 {
  86         this->path_.clear();
  87         while (g != nullptr) {
  88                 this->path_.push_back(g);
  89                 g = g->p();
  90         }
  91         std::reverse(this->path_.begin(), this->path_.end());
  92         for (unsigned int i = 1; i < this->path_.size(); i++) {
  93                 this->path_[i]->c(this->cost_build(*this->path_[i - 1],
  94                         *this->path_[i]));
  95         }
  96 }
  97
  98 void
  99 RRTS::recompute_path_cc()
 100 {
 101         this->recompute_cc(&this->goal_);
 102 }
 103
 104 double
 105 RRTS::min_gamma_eta() const
 106 {
 107         double ns = this->nodes_.size();
 108         double gamma = pow(log(ns) / ns, 1.0 / 3.0);
 109         return std::min(gamma, this->eta_);
 110 }
 111
 112 bool
 113 RRTS::should_continue() const
 114 {
 115         return !this->should_finish();
 116 }
 117
 118 void
 119 RRTS::join_steered(RRTNode* f)
 120 {
 121         while (this->steered_.size() > 0) {
 122                 this->store(this->steered_.front());
 123                 RRTNode* t = &this->nodes_.back();
 124                 t->p(*f);
 125                 t->c(this->cost_build(*f, *t));
 126                 t->cusp(*f);
 127                 this->steered_.erase(this->steered_.begin());
 128                 f = t;
 129         }
 130 }
 131
 132 RRTNode&
 133 RRTS::nn()
 134 {
 135         return *this->nn_;
 136 }
 137
 138 bool
 139 RRTS::connect()
 140 {
 141         RRTNode* f = this->nn_;
 142         RRTNode* t = &this->steered_.front();
 143         double cost = f->cc() + this->cost_build(*f, *t);
 144         for (auto n: this->nv_) {
 145                 double nc = n->cc() + this->cost_build(*n, *t);
 146                 if (nc < cost) {
 147                         f = n;
 148                         cost = nc;
 149                 }
 150         }
 151         // Check if it's possible to drive from *f to *t. If not, then fallback
 152         // to *f = nn_. This could be also solved by additional steer from *f to
 153         // *t instead of the following code.
 154         this->bc_.set_pose(*f);
 155         if (!this->bc_.drivable(*t)) {
 156                 f = this->nn_;
 157         }
 158         this->store(this->steered_.front());
 159         t = &this->nodes_.back();
 160         t->p(*f);
 161         t->c(this->cost_build(*f, *t));
 162         t->cusp(*f);
 163         this->steered_.erase(this->steered_.begin());
 164         return true;
 165 }
 166
 167 void
 168 RRTS::rewire()
 169 {
 170         RRTNode *f = &this->nodes_.back();
 171         for (auto n: this->nv_) {
 172                 double fc = f->cc() + this->cost_build(*f, *n);
 173                 this->bc_.set_pose(*f);
 174                 bool drivable = this->bc_.drivable(*n);
 175                 if (drivable && fc < n->cc()) {
 176                         n->p(*f);
 177                         n->c(this->cost_build(*f, *n));
 178                 }
 179         }
 180 }
 181
 182 bool
 183 RRTS::goal_drivable_from(RRTNode const& f)
 184 {
 185         this->bc_.set_pose(f);
 186         return this->bc_.drivable(this->goal_);
 187 }
 188
 189 void
 190 RRTS::store(RRTNode n)
 191 {
 192         this->nodes_.push_back(n);
 193 }
 194
 195 double
 196 RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
 197 {
 198         return f.edist(t);
 199 }
 200
 201 double
 202 RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
 203 {
 204         return this->cost_build(f, t);
 205 }
 206
 207 void
 208 RRTS::find_nn(RRTNode const& t)
 209 {
 210         this->nn_ = &this->nodes_.front();
 211         this->cost_ = this->cost_search(*this->nn_, t);
 212         for (auto& f: this->nodes_) {
 213                 if (this->cost_search(f, t) < this->cost_) {
 214                         this->nn_ = &f;
 215                         this->cost_ = this->cost_search(f, t);
 216                 }
 217         }
 218 }
 219
 220 void
 221 RRTS::find_nv(RRTNode const& t)
 222 {
 223         this->nv_.clear();
 224         this->cost_ = this->min_gamma_eta();
 225         for (auto& f: this->nodes_) {
 226                 if (this->cost_search(f, t) < this->cost_) {
 227                         this->nv_.push_back(&f);
 228                 }
 229         }
 230 }
 231
 232 void
 233 RRTS::compute_path()
 234 {
 235         this->path_.clear();
 236         RRTNode *g = &this->goal_;
 237         if (g->p() == nullptr) {
 238                 return;
 239         }
 240         while (g != nullptr && this->path_.size() < 10000) {
 241                 /* FIXME in ext13
 242                  *
 243                  * There shouldn't be this->path_.size() < 10000 condition.
 244                  * However, the RRTS::compute_path() called from
 245                  * RRTExt13::compute_path tends to re-allocate this->path_
 246                  * infinitely. There's probably node->p() = &node somewhere...
 247                  */
 248                 this->path_.push_back(g);
 249                 g = g->p();
 250         }
 251         std::reverse(this->path_.begin(), this->path_.end());
 252 }
 253
 254 RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
 255 {
 256         this->nodes_.reserve(4000000);
 257         this->steered_.reserve(1000);
 258         this->path_.reserve(10000);
 259         this->nv_.reserve(1000);
 260         this->store(RRTNode()); // root
 261 }
 262
 263 unsigned int
 264 RRTS::icnt() const
 265 {
 266         return this->icnt_;
 267 }
 268
 269 void
 270 RRTS::icnt(unsigned int i)
 271 {
 272         this->icnt_ = i;
 273 }
 274
 275 double
 276 RRTS::scnt() const
 277 {
 278         return this->ter_.scnt();
 279 }
 280
 281 Json::Value
 282 RRTS::json() const
 283 {
 284         Json::Value jvo;
 285         unsigned int i = 0;
 286         for (auto n: this->path_) {
 287                 jvo["path"][i][0] = n->x();
 288                 jvo["path"][i][1] = n->y();
 289                 jvo["path"][i][2] = n->h();
 290                 i++;
 291         }
 292         jvo["goal_cc"] = this->goal_.cc();
 293         jvo["time"] = this->time_;
 294         return jvo;
 295 }
 296
 297 void
 298 RRTS::json(Json::Value jvi)
 299 {
 300         assert(jvi["init"] != Json::nullValue);
 301         assert(jvi["goal"] != Json::nullValue);
 302         this->nodes_.front().x(jvi["init"][0].asDouble());
 303         this->nodes_.front().y(jvi["init"][1].asDouble());
 304         this->nodes_.front().h(jvi["init"][2].asDouble());
 305         if (jvi["goal"].size() == 4) {
 306                 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
 307                         jvi["goal"][1].asDouble(),
 308                         jvi["goal"][2].asDouble(),
 309                         jvi["goal"][3].asDouble());
 310         } else {
 311                 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
 312                         jvi["goal"][1].asDouble(),
 313                         jvi["goal"][2].asDouble(),
 314                         jvi["goal"][2].asDouble());
 315         }
 316 }
 317
 318 bool
 319 RRTS::next()
 320 {
 321         if (this->icnt_ == 0) {
 322                 this->ter_.start();
 323         }
 324         this->icnt_ += 1;
 325         auto rs = this->sample();
 326 #if 1 // anytime RRTs
 327 {
 328         double d1 = this->cost_search(this->nodes_.front(), rs);
 329         double d2 = this->cost_search(rs, this->goal_);
 330         if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
 331                 rs = this->last_path_[rand() % this->last_path_.size()];
 332         }
 333 }
 334 #endif
 335         this->find_nn(rs);
 336         this->steer(this->nn(), rs);
 337         if (this->collide_steered()) {
 338                 return this->should_continue();
 339         }
 340         this->find_nv(this->steered_.front());
 341         if (!this->connect()) {
 342                 return this->should_continue();
 343         }
 344         this->rewire();
 345         unsigned int ss = this->steered_.size();
 346         this->join_steered(&this->nodes_.back());
 347         RRTNode* just_added = &this->nodes_.back();
 348         bool gf = false;
 349         while (ss > 0 && just_added->p() != nullptr) {
 350                 this->steer(*just_added, this->goal_);
 351                 if (this->collide_steered()) {
 352                         ss--;
 353                         just_added = just_added->p();
 354                         continue;
 355                 }
 356                 this->join_steered(just_added);
 357                 bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
 358                 bool gd = this->goal_drivable_from(this->nodes_.back());
 359                 if (gn && gd) {
 360                         double nc = this->cost_build(this->nodes_.back(),
 361                                 this->goal_);
 362                         double ncc = this->nodes_.back().cc() + nc;
 363                         if (this->goal_.p() == nullptr
 364                                         || ncc < this->goal_.cc()) {
 365                                 this->goal_.p(this->nodes_.back());
 366                                 this->goal_.c(nc);
 367                                 gf = true;
 368                         }
 369                 }
 370                 ss--;
 371                 just_added = just_added->p();
 372         }
 373         if (gf) {
 374                 this->compute_path();
 375         }
 376         this->time_ = this->ter_.scnt();
 377         return this->should_continue();
 378 }
 379
 380 void
 381 RRTS::reset()
 382 {
 383         if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
 384                 this->last_goal_cc_ = this->goal_.cc();
 385                 this->last_path_.clear();
 386                 for (auto n: this->path_) {
 387                         this->last_path_.push_back(*n);
 388                 }
 389         }
 390         this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
 391                 this->goal_.e());
 392         this->path_.clear();
 393         this->steered_.clear();
 394         this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());
 395         this->nv_.clear();
 396         this->nn_ = nullptr;
 397 }
 398
 399 } // namespace rrts