2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
12 #define USE_RRTS 0 // TODO improve, this solution isn't clear.
20 this->_tstart = std::chrono::high_resolution_clock::now();
26 auto t = std::chrono::high_resolution_clock::now() - this->_tstart;
27 auto d = std::chrono::duration_cast<std::chrono::seconds>(t);
40 assert(this->_p != nullptr);
42 this->_cc = this->_p->cc() + c;
58 RRTNode::p(RRTNode& p)
72 RRTNode::cusp(RRTNode const& p)
74 this->_cusp = p.cusp();
75 if (this->sp() != p.sp() || this->sp() == 0.0) {
83 return this->_segment_type;
89 this->_segment_type = st;
93 RRTNode::operator==(RRTNode const& n)
99 RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
101 assert(this->_path.size() == 0);
102 while (g != nullptr) {
103 this->_path.push_back(g);
106 std::reverse(this->_path.begin(), this->_path.end());
107 for (unsigned int i = 1; i < this->_path.size(); i++) {
108 this->_path[i]->c(this->cost_build(
116 RRTS::recompute_path_cc()
118 this->recompute_cc_for_predecessors_and(&this->_goal);
122 RRTS::min_gamma_eta() const
124 double ns = this->_nodes.size();
125 double gamma = pow(log(ns) / ns, 1.0 / 3.0);
126 return std::min(gamma, this->eta());
130 RRTS::should_continue() const
132 return !this->should_finish();
136 RRTS::join_steered(RRTNode* f)
138 while (this->_steered.size() > 0) {
139 this->store(this->_steered.front());
140 RRTNode* t = &this->_nodes.back();
142 t->c(this->cost_build(*f, *t));
144 this->_steered.erase(this->_steered.begin());
152 RRTNode* f = this->_nn;
153 RRTNode* t = &this->_steered.front();
155 double cost = f->cc() + this->cost_build(*f, *t);
156 for (auto n: this->nv_) {
157 double nc = n->cc() + this->cost_build(*n, *t);
163 // Check if it's possible to drive from *f to *t. If not, then fallback
164 // to *f = _nn. This could be also solved by additional steer from *f to
165 // *t instead of the following code.
166 this->set_bc_pose_to(*f);
167 if (!this->_bc.drivable(*t)) {
171 this->store(this->_steered.front());
172 t = &this->_nodes.back();
174 t->c(this->cost_build(*f, *t));
176 this->_steered.erase(this->_steered.begin());
183 RRTNode *f = &this->_nodes.back();
184 for (auto n: this->_nv) {
185 double fc = f->cc() + this->cost_build(*f, *n);
186 this->set_bc_pose_to(*f);
187 bool drivable = this->_bc.drivable(*n);
188 if (drivable && fc < n->cc()) {
190 n->c(this->cost_build(*f, *n));
196 RRTS::goal_drivable_from(RRTNode const& f)
198 this->set_bc_pose_to(f);
199 return this->_bc.drivable(this->_goal);
203 RRTS::store(RRTNode n)
205 this->_nodes.push_back(n);
209 RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
215 RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
217 return this->cost_build(f, t);
221 RRTS::find_nn(RRTNode const& t)
223 this->_nn = &this->_nodes.front();
224 this->_cost = this->cost_search(*this->_nn, t);
225 for (auto& f: this->_nodes) {
226 if (this->cost_search(f, t) < this->_cost) {
228 this->_cost = this->cost_search(f, t);
234 RRTS::find_nv(RRTNode const& t)
237 this->_cost = this->min_gamma_eta();
238 for (auto& f: this->_nodes) {
239 if (this->cost_search(f, t) < this->_cost) {
240 this->_nv.push_back(&f);
249 RRTNode *g = &this->_goal;
250 if (g->p() == nullptr) {
253 while (g != nullptr && this->_path.size() < 10000) {
256 * There shouldn't be this->_path.size() < 10000 condition.
257 * However, the RRTS::compute_path() called from
258 * RRTExt13::compute_path tends to re-allocate this->_path
259 * infinitely. There's probably node->p() = &node somewhere...
261 this->_path.push_back(g);
264 std::reverse(this->_path.begin(), this->_path.end());
267 RRTS::RRTS() : _goal(0.0, 0.0, 0.0, 0.0), _gen(std::random_device{}())
269 this->_nodes.reserve(4000000);
270 this->_steered.reserve(1000);
271 this->_path.reserve(10000);
272 this->_nv.reserve(1000);
273 this->store(RRTNode()); // root
277 RRTS::set_bc_pose_to(Pose const& p)
279 this->_bc.set_pose_to(p);
283 RRTS::goal(void) const
289 RRTS::goal(double x, double y, double b, double e)
291 this->_goal = RRTGoal(x, y, b, e);
295 RRTS::icnt(void) const
301 RRTS::icnt(unsigned int i)
307 RRTS::icnt_max(void) const
309 return this->_icnt_max;
313 RRTS::icnt_max(unsigned int i)
327 return this->_ter.scnt();
331 RRTS::start(double x, double y, double h)
333 this->_nodes.front().x(x);
334 this->_nodes.front().y(y);
335 this->_nodes.front().h(h);
341 std::vector<Pose> path;
342 for (auto n: this->_path) {
343 path.push_back(Pose(n->x(), n->y(), n->h()));
349 RRTS::path_cost() const
351 return this->_goal.cc();
355 RRTS::last_path_cost(void) const
357 if (this->_logged_paths.size() == 0) {
360 assert(this->_logged_paths.back().size() > 0);
361 return this->_logged_paths.back().back().cc();
377 RRTS::json(void) const
380 unsigned int i = 0, j = 0;
381 for (auto path: this->_logged_paths) {
384 jvo["paths"][j][i][0] = n.x();
385 jvo["paths"][j][i][1] = n.y();
386 jvo["paths"][j][i][2] = n.h();
387 jvo["paths"][j][i][3] = n.sp();
388 jvo["paths"][j][i][4] = n.st();
391 jvo["costs"][j] = path.back().cc();
395 for (auto n: this->_path) {
396 jvo["paths"][j][i][0] = n->x();
397 jvo["paths"][j][i][1] = n->y();
398 jvo["paths"][j][i][2] = n->h();
399 jvo["paths"][j][i][3] = n->sp();
400 jvo["paths"][j][i][4] = n->st();
401 jvo["path"][i][0] = n->x();
402 jvo["path"][i][1] = n->y();
403 jvo["path"][i][2] = n->h();
404 jvo["path"][i][3] = n->sp();
405 jvo["path"][i][4] = n->st();
408 jvo["costs"][j] = this->_path.back()->cc();
410 jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
411 jvo["cost"] = this->path_cost();
412 jvo["time"] = this->scnt();
417 RRTS::json(Json::Value jvi)
419 assert(jvi["init"] != Json::nullValue);
420 assert(jvi["goal"] != Json::nullValue);
422 jvi["init"][0].asDouble(),
423 jvi["init"][1].asDouble(),
424 jvi["init"][2].asDouble());
425 if (jvi["goal"].size() == 4) {
427 jvi["goal"][0].asDouble(),
428 jvi["goal"][1].asDouble(),
429 jvi["goal"][2].asDouble(),
430 jvi["goal"][3].asDouble());
433 jvi["goal"][0].asDouble(),
434 jvi["goal"][1].asDouble(),
435 jvi["goal"][2].asDouble(),
436 jvi["goal"][2].asDouble());
443 if (this->icnt() == 0) {
446 auto rs = this->sample();
447 #if 1 // anytime RRTs
449 double d1 = this->cost_search(this->_nodes.front(), rs);
450 double d2 = this->cost_search(rs, this->_goal);
451 if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
452 auto& last_path = this->_logged_paths.back();
453 rs = last_path[rand() % last_path.size()];
458 this->steer(*this->_nn, rs);
459 if (this->collide_steered()) {
460 return this->should_continue();
463 this->find_nv(this->_steered.front());
465 if (!this->connect()) {
466 return this->should_continue();
471 unsigned int ss = this->_steered.size();
472 this->join_steered(&this->_nodes.back());
473 RRTNode* just_added = &this->_nodes.back();
475 while (ss > 0 && just_added->p() != nullptr) {
476 this->steer(*just_added, this->_goal);
477 if (this->collide_steered()) {
479 just_added = just_added->p();
482 this->join_steered(just_added);
483 bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
484 bool gd = this->goal_drivable_from(this->_nodes.back());
486 double nc = this->cost_build(
489 double ncc = this->_nodes.back().cc() + nc;
490 if (this->_goal.p() == nullptr
491 || ncc < this->_goal.cc()) {
492 this->_goal.p(this->_nodes.back());
498 just_added = just_added->p();
501 this->compute_path();
503 this->_time = this->scnt();
504 this->icnt(this->icnt() + 1);
505 return this->should_continue();
511 if (this->path_cost() != 0.0
512 && this->path_cost() < this->last_path_cost()) {
513 this->_logged_paths.push_back(std::vector<RRTNode>());
514 auto& last_path = this->_logged_paths.back();
515 last_path.reserve(this->_path.size());
516 RRTNode* p = nullptr;
517 for (auto n: this->_path) {
518 last_path.push_back(*n);
520 last_path.back().p(*p);
522 p = &last_path.back();
524 // Test that last path cost matches.
525 auto last_path_cost = last_path.back().cc();
526 for (unsigned int i = 1; i < last_path.size(); i++) {
527 last_path[i].c(this->cost_build(
531 assert(last_path_cost == last_path.back().cc());
533 this->_goal = RRTGoal(
539 this->_steered.clear();
540 this->_nodes.erase(this->_nodes.begin() + 1, this->_nodes.end());
543 this->_bc = BicycleCar();