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