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