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