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