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::min_gamma_eta() const
65 double ns = this->nodes_.size();
66 double gamma = pow(log(ns) / ns, 1.0 / 3.0);
67 return std::min(gamma, this->eta_);
71 RRTS::should_continue() const
73 return !this->should_finish();
77 RRTS::join_steered(RRTNode* f)
79 while (this->steered_.size() > 0) {
80 this->store(this->steered_.front());
81 RRTNode* t = &this->nodes_.back();
83 t->c(this->cost_build(*f, *t));
84 this->steered_.erase(this->steered_.begin());
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);
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)) {
115 this->store(this->steered_.front());
116 t = &this->nodes_.back();
118 t->c(this->cost_build(*f, *t));
119 this->steered_.erase(this->steered_.begin());
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()) {
133 n->c(this->cost_build(*f, *n));
139 RRTS::goal_drivable_from(RRTNode const& f)
141 this->bc_.set_pose(f);
142 return this->bc_.drivable(this->goal_);
146 RRTS::store(RRTNode n)
148 this->nodes_.push_back(n);
152 RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
158 RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
160 return this->cost_build(f, t);
164 RRTS::find_nn(RRTNode const& t)
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_) {
171 this->cost_ = this->cost_search(f, t);
177 RRTS::find_nv(RRTNode const& t)
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);
192 RRTNode *g = &this->goal_;
193 if (g->p() == nullptr) {
196 while (g != nullptr && this->path_.size() < 10000) {
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...
204 this->path_.push_back(g);
207 std::reverse(this->path_.begin(), this->path_.end());
210 RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
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
226 RRTS::icnt(unsigned int i)
234 return this->ter_.scnt();
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();
248 jvo["goal_cc"] = this->goal_.cc();
249 jvo["time"] = this->time_;
254 RRTS::json(Json::Value jvi)
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());
268 this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
269 jvi["goal"][1].asDouble(),
270 jvi["goal"][2].asDouble(),
271 jvi["goal"][2].asDouble());
278 if (this->icnt_ == 0) {
282 auto rs = this->sample();
283 #if 1 // anytime RRTs
285 double d1 = this->cost_search(this->nodes_.front(), rs);
286 double d2 = this->cost_search(rs, this->goal_);
287 if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
289 return this->should_continue();
294 this->steer(this->nn(), rs);
295 if (this->collide_steered()) {
296 return this->should_continue();
298 this->find_nv(this->steered_.front());
299 if (!this->connect()) {
300 return this->should_continue();
303 unsigned int ss = this->steered_.size();
304 this->join_steered(&this->nodes_.back());
305 RRTNode* just_added = &this->nodes_.back();
307 while (ss > 0 && just_added->p() != nullptr) {
308 this->steer(*just_added, this->goal_);
309 if (this->collide_steered()) {
311 just_added = just_added->p();
314 this->join_steered(just_added);
315 bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
316 bool gd = this->goal_drivable_from(this->nodes_.back());
318 double nc = this->cost_build(this->nodes_.back(),
320 double ncc = this->nodes_.back().cc() + nc;
321 if (this->goal_.p() == nullptr
322 || ncc < this->goal_.cc()) {
323 this->goal_.p(this->nodes_.back());
329 just_added = just_added->p();
331 #if 1 // test if root -> just_added can be steered directly
332 this->steer(this->nodes_.front(), *just_added);
333 ss = this->steered_.size();
334 if (!this->collide_steered() && this->steered_.size() == ss) {
335 this->join_steered(&this->nodes_.front());
336 just_added->p(this->nodes_.back());
337 just_added->c(this->cost_build(this->nodes_.back(),
342 this->compute_path();
344 this->time_ = this->ter_.scnt();
345 return this->should_continue();
351 if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
352 this->last_goal_cc_ = this->goal_.cc();
354 this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
357 this->steered_.clear();
358 this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());