src/rrts.cc

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