10 this->tstart_ = std::chrono::high_resolution_clock::now();
16 using namespace std::chrono;
17 auto t = high_resolution_clock::now() - this->tstart_;
18 auto d = duration_cast<duration<double>>(t);
31 assert(this->p_ != nullptr);
33 this->cc_ = this->p_->cc() + c;
49 RRTNode::p(RRTNode& p)
57 RRTNode::operator==(RRTNode const& n)
63 RRTS::recompute_path_cc()
66 RRTNode* g = &this->goal_;
67 while (g != nullptr) {
68 this->path_.push_back(g);
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],
79 RRTS::min_gamma_eta() const
81 double ns = this->nodes_.size();
82 double gamma = pow(log(ns) / ns, 1.0 / 3.0);
83 return std::min(gamma, this->eta_);
87 RRTS::should_continue() const
89 return !this->should_finish();
93 RRTS::join_steered(RRTNode* f)
95 while (this->steered_.size() > 0) {
96 this->store(this->steered_.front());
97 RRTNode* t = &this->nodes_.back();
99 t->c(this->cost_build(*f, *t));
100 this->steered_.erase(this->steered_.begin());
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);
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)) {
131 this->store(this->steered_.front());
132 t = &this->nodes_.back();
134 t->c(this->cost_build(*f, *t));
135 this->steered_.erase(this->steered_.begin());
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()) {
149 n->c(this->cost_build(*f, *n));
155 RRTS::goal_drivable_from(RRTNode const& f)
157 this->bc_.set_pose(f);
158 return this->bc_.drivable(this->goal_);
162 RRTS::store(RRTNode n)
164 this->nodes_.push_back(n);
168 RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
174 RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
176 return this->cost_build(f, t);
180 RRTS::find_nn(RRTNode const& t)
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_) {
187 this->cost_ = this->cost_search(f, t);
193 RRTS::find_nv(RRTNode const& t)
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);
208 RRTNode *g = &this->goal_;
209 if (g->p() == nullptr) {
212 while (g != nullptr && this->path_.size() < 10000) {
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...
220 this->path_.push_back(g);
223 std::reverse(this->path_.begin(), this->path_.end());
226 RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
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
242 RRTS::icnt(unsigned int i)
250 return this->ter_.scnt();
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();
264 jvo["goal_cc"] = this->goal_.cc();
265 jvo["time"] = this->time_;
270 RRTS::json(Json::Value jvi)
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());
284 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
285 jvi["goal"][1].asDouble(),
286 jvi["goal"][2].asDouble(),
287 jvi["goal"][2].asDouble());
294 if (this->icnt_ == 0) {
298 auto rs = this->sample();
299 #if 1 // anytime RRTs
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 rs = this->last_path_[rand() % this->last_path_.size()];
309 this->steer(this->nn(), rs);
310 if (this->collide_steered()) {
311 return this->should_continue();
313 this->find_nv(this->steered_.front());
314 if (!this->connect()) {
315 return this->should_continue();
318 unsigned int ss = this->steered_.size();
319 this->join_steered(&this->nodes_.back());
320 RRTNode* just_added = &this->nodes_.back();
322 while (ss > 0 && just_added->p() != nullptr) {
323 this->steer(*just_added, this->goal_);
324 if (this->collide_steered()) {
326 just_added = just_added->p();
329 this->join_steered(just_added);
330 bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
331 bool gd = this->goal_drivable_from(this->nodes_.back());
333 double nc = this->cost_build(this->nodes_.back(),
335 double ncc = this->nodes_.back().cc() + nc;
336 if (this->goal_.p() == nullptr
337 || ncc < this->goal_.cc()) {
338 this->goal_.p(this->nodes_.back());
344 just_added = just_added->p();
347 this->compute_path();
349 this->time_ = this->ter_.scnt();
350 return this->should_continue();
356 if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
357 this->last_goal_cc_ = this->goal_.cc();
358 this->last_path_.clear();
359 for (auto n: this->path_) {
360 this->last_path_.push_back(*n);
363 this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
366 this->steered_.clear();
367 this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());