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{}()), goal_(0.0, 0.0, 0.0, 0.0)
 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         if (jvi["goal"].size() == 4) {
 263                 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
 264                         jvi["goal"][1].asDouble(),
 265                         jvi["goal"][2].asDouble(),
 266                         jvi["goal"][3].asDouble());
 267         } else {
 268                 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
 269                         jvi["goal"][1].asDouble(),
 270                         jvi["goal"][2].asDouble(),
 271                         jvi["goal"][2].asDouble());
 272         }
 273 }
 274
 275 bool
 276 RRTS::next()
 277 {
 278         if (this->icnt_ == 0) {
 279                 this->ter_.start();
 280         }
 281         this->icnt_ += 1;
 282         auto rs = this->sample();
 283         this->find_nn(rs);
 284         this->steer(this->nn(), rs);
 285         if (this->collide_steered()) {
 286                 return this->should_continue();
 287         }
 288         this->find_nv(this->steered_.front());
 289         if (!this->connect()) {
 290                 return this->should_continue();
 291         }
 292         this->rewire();
 293         unsigned int ss = this->steered_.size();
 294         this->join_steered(&this->nodes_.back());
 295         RRTNode* just_added = &this->nodes_.back();
 296         while (ss > 0 && just_added->p() != nullptr) {
 297                 //if (!this->goal_drivable_from(*just_added)) {
 298                 //      ss--;
 299                 //      just_added = just_added->p();
 300                 //      continue;
 301                 //}
 302                 this->steer(*just_added, this->goal_);
 303                 if (this->collide_steered()) {
 304                         ss--;
 305                         just_added = just_added->p();
 306                         continue;
 307                 }
 308                 this->join_steered(just_added);
 309                 bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
 310                 bool gd = this->goal_drivable_from(this->nodes_.back());
 311                 if (gn && gd) {
 312                         double nc = this->cost_build(this->nodes_.back(),
 313                                 this->goal_);
 314                         double ncc = this->nodes_.back().cc() + nc;
 315                         if (this->goal_.p() == nullptr
 316                                         || ncc < this->goal_.cc()) {
 317                                 this->goal_.p(this->nodes_.back());
 318                                 this->goal_.c(nc);
 319                                 this->compute_path();
 320                         }
 321                 }
 322                 ss--;
 323                 just_added = just_added->p();
 324         }
 325
 326         ////if (!this->goal_drivable_from(this->nodes_.back())) {
 327         ////    return this->should_continue();
 328         ////}
 329         //this->steer(this->nodes_.back(), this->goal_);
 330         //if (this->collide_steered()) {
 331         //      return this->should_continue();
 332         //}
 333         //this->join_steered(&this->nodes_.back());
 334         //bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
 335         //bool gd = this->goal_drivable_from(this->nodes_.back());
 336         //if (gn && gd) {
 337         //      double nc = this->cost_build(this->nodes_.back(), this->goal_);
 338         //      double ncc = this->nodes_.back().cc() + nc;
 339         //      if (this->goal_.p() == nullptr || ncc < this->goal_.cc()) {
 340         //              this->goal_.p(this->nodes_.back());
 341         //              this->goal_.c(nc);
 342         //              this->compute_path();
 343         //      }
 344         //}
 345         this->time_ = this->ter_.scnt();
 346         return this->should_continue();
 347 }
 348
 349 void
 350 RRTS::reset()
 351 {
 352         this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
 353                 this->goal_.e());
 354         this->path_.clear();
 355         this->steered_.clear();
 356         this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());
 357         this->nv_.clear();
 358         this->nn_ = nullptr;
 359 }
 360
 361 } // namespace rrts