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() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
 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 unsigned int
 270 RRTS::icnt() const
 271 {
 272         return this->icnt_;
 273 }
 274
 275 void
 276 RRTS::icnt(unsigned int i)
 277 {
 278         this->icnt_ = i;
 279 }
 280
 281 double
 282 RRTS::scnt() const
 283 {
 284         return this->ter_.scnt();
 285 }
 286
 287 Json::Value
 288 RRTS::json() const
 289 {
 290         Json::Value jvo;
 291         unsigned int i = 0;
 292         for (auto n: this->path_) {
 293                 jvo["path"][i][0] = n->x();
 294                 jvo["path"][i][1] = n->y();
 295                 jvo["path"][i][2] = n->h();
 296                 i++;
 297         }
 298         jvo["goal_cc"] = this->goal_.cc();
 299         jvo["time"] = this->time_;
 300         return jvo;
 301 }
 302
 303 void
 304 RRTS::json(Json::Value jvi)
 305 {
 306         assert(jvi["init"] != Json::nullValue);
 307         assert(jvi["goal"] != Json::nullValue);
 308         this->nodes_.front().x(jvi["init"][0].asDouble());
 309         this->nodes_.front().y(jvi["init"][1].asDouble());
 310         this->nodes_.front().h(jvi["init"][2].asDouble());
 311         if (jvi["goal"].size() == 4) {
 312                 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
 313                         jvi["goal"][1].asDouble(),
 314                         jvi["goal"][2].asDouble(),
 315                         jvi["goal"][3].asDouble());
 316         } else {
 317                 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
 318                         jvi["goal"][1].asDouble(),
 319                         jvi["goal"][2].asDouble(),
 320                         jvi["goal"][2].asDouble());
 321         }
 322 }
 323
 324 bool
 325 RRTS::next()
 326 {
 327         if (this->icnt_ == 0) {
 328                 this->ter_.start();
 329         }
 330         this->icnt_ += 1;
 331         auto rs = this->sample();
 332 #if 1 // anytime RRTs
 333 {
 334         double d1 = this->cost_search(this->nodes_.front(), rs);
 335         double d2 = this->cost_search(rs, this->goal_);
 336         if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
 337                 rs = this->last_path_[rand() % this->last_path_.size()];
 338         }
 339 }
 340 #endif
 341         this->find_nn(rs);
 342         this->steer(this->nn(), rs);
 343         if (this->collide_steered()) {
 344                 return this->should_continue();
 345         }
 346 #if USE_RRTS
 347         this->find_nv(this->steered_.front());
 348 #endif
 349         if (!this->connect()) {
 350                 return this->should_continue();
 351         }
 352 #if USE_RRTS
 353         this->rewire();
 354 #endif
 355         unsigned int ss = this->steered_.size();
 356         this->join_steered(&this->nodes_.back());
 357         RRTNode* just_added = &this->nodes_.back();
 358         bool gf = false;
 359         while (ss > 0 && just_added->p() != nullptr) {
 360                 this->steer(*just_added, this->goal_);
 361                 if (this->collide_steered()) {
 362                         ss--;
 363                         just_added = just_added->p();
 364                         continue;
 365                 }
 366                 this->join_steered(just_added);
 367                 bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
 368                 bool gd = this->goal_drivable_from(this->nodes_.back());
 369                 if (gn && gd) {
 370                         double nc = this->cost_build(this->nodes_.back(),
 371                                 this->goal_);
 372                         double ncc = this->nodes_.back().cc() + nc;
 373                         if (this->goal_.p() == nullptr
 374                                         || ncc < this->goal_.cc()) {
 375                                 this->goal_.p(this->nodes_.back());
 376                                 this->goal_.c(nc);
 377                                 gf = true;
 378                         }
 379                 }
 380                 ss--;
 381                 just_added = just_added->p();
 382         }
 383         if (gf) {
 384                 this->compute_path();
 385         }
 386         this->time_ = this->ter_.scnt();
 387         return this->should_continue();
 388 }
 389
 390 void
 391 RRTS::reset()
 392 {
 393         if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
 394                 this->last_goal_cc_ = this->goal_.cc();
 395                 this->last_path_.clear();
 396                 for (auto n: this->path_) {
 397                         this->last_path_.push_back(*n);
 398                 }
 399         }
 400         this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
 401                 this->goal_.e());
 402         this->path_.clear();
 403         this->steered_.clear();
 404         this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());
 405         this->nv_.clear();
 406         this->nn_ = nullptr;
 407 }
 408
 409 } // namespace rrts