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 std::vector<Pose>
 296 RRTS::get_path() const
 297 {
 298         std::vector<Pose> path;
 299         for (auto n: this->path_) {
 300                 path.push_back(Pose(n->x(), n->y(), n->h()));
 301         }
 302         return path;
 303 }
 304
 305 double
 306 RRTS::get_path_cost() const
 307 {
 308         return this->goal_.cc();
 309 }
 310
 311 unsigned int
 312 RRTS::icnt() const
 313 {
 314         return this->icnt_;
 315 }
 316
 317 void
 318 RRTS::icnt(unsigned int i)
 319 {
 320         this->icnt_ = i;
 321 }
 322
 323 double
 324 RRTS::scnt() const
 325 {
 326         return this->ter_.scnt();
 327 }
 328
 329 Json::Value
 330 RRTS::json() const
 331 {
 332         Json::Value jvo;
 333         unsigned int i = 0;
 334         for (auto n: this->path_) {
 335                 jvo["path"][i][0] = n->x();
 336                 jvo["path"][i][1] = n->y();
 337                 jvo["path"][i][2] = n->h();
 338                 i++;
 339         }
 340         jvo["goal_cc"] = this->goal_.cc();
 341         jvo["time"] = this->time_;
 342         return jvo;
 343 }
 344
 345 void
 346 RRTS::json(Json::Value jvi)
 347 {
 348         assert(jvi["init"] != Json::nullValue);
 349         assert(jvi["goal"] != Json::nullValue);
 350         this->nodes_.front().x(jvi["init"][0].asDouble());
 351         this->nodes_.front().y(jvi["init"][1].asDouble());
 352         this->nodes_.front().h(jvi["init"][2].asDouble());
 353         if (jvi["goal"].size() == 4) {
 354                 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
 355                         jvi["goal"][1].asDouble(),
 356                         jvi["goal"][2].asDouble(),
 357                         jvi["goal"][3].asDouble());
 358         } else {
 359                 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
 360                         jvi["goal"][1].asDouble(),
 361                         jvi["goal"][2].asDouble(),
 362                         jvi["goal"][2].asDouble());
 363         }
 364 }
 365
 366 bool
 367 RRTS::next()
 368 {
 369         if (this->icnt_ == 0) {
 370                 this->ter_.start();
 371         }
 372         this->icnt_ += 1;
 373         auto rs = this->sample();
 374 #if 1 // anytime RRTs
 375 {
 376         double d1 = this->cost_search(this->nodes_.front(), rs);
 377         double d2 = this->cost_search(rs, this->goal_);
 378         if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
 379                 rs = this->last_path_[rand() % this->last_path_.size()];
 380         }
 381 }
 382 #endif
 383         this->find_nn(rs);
 384         this->steer(this->nn(), rs);
 385         if (this->collide_steered()) {
 386                 return this->should_continue();
 387         }
 388 #if USE_RRTS
 389         this->find_nv(this->steered_.front());
 390 #endif
 391         if (!this->connect()) {
 392                 return this->should_continue();
 393         }
 394 #if USE_RRTS
 395         this->rewire();
 396 #endif
 397         unsigned int ss = this->steered_.size();
 398         this->join_steered(&this->nodes_.back());
 399         RRTNode* just_added = &this->nodes_.back();
 400         bool gf = false;
 401         while (ss > 0 && just_added->p() != nullptr) {
 402                 this->steer(*just_added, this->goal_);
 403                 if (this->collide_steered()) {
 404                         ss--;
 405                         just_added = just_added->p();
 406                         continue;
 407                 }
 408                 this->join_steered(just_added);
 409                 bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
 410                 bool gd = this->goal_drivable_from(this->nodes_.back());
 411                 if (gn && gd) {
 412                         double nc = this->cost_build(this->nodes_.back(),
 413                                 this->goal_);
 414                         double ncc = this->nodes_.back().cc() + nc;
 415                         if (this->goal_.p() == nullptr
 416                                         || ncc < this->goal_.cc()) {
 417                                 this->goal_.p(this->nodes_.back());
 418                                 this->goal_.c(nc);
 419                                 gf = true;
 420                         }
 421                 }
 422                 ss--;
 423                 just_added = just_added->p();
 424         }
 425         if (gf) {
 426                 this->compute_path();
 427         }
 428         this->time_ = this->ter_.scnt();
 429         return this->should_continue();
 430 }
 431
 432 void
 433 RRTS::reset()
 434 {
 435         if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
 436                 this->last_goal_cc_ = this->goal_.cc();
 437                 this->last_path_.clear();
 438                 for (auto n: this->path_) {
 439                         this->last_path_.push_back(*n);
 440                 }
 441         }
 442         this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
 443                 this->goal_.e());
 444         this->path_.clear();
 445         this->steered_.clear();
 446         this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());
 447         this->nv_.clear();
 448         this->nn_ = nullptr;
 449 }
 450
 451 } // namespace rrts