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, bool can_be_too_close)
61 if (!can_be_too_close) {
62 assert(!(std::abs(p.x() - this->x()) < 1e-3
63 && std::abs(p.y() - this->y()) < 1e-3
64 && std::abs(p.h() - this->h()) < 1e-3));
67 this->p_is_cusp(this->would_be_cusp_if_parent(p));
72 RRTNode::p(RRTNode& p)
74 return this->p(p, false);
78 RRTNode::cusp_cnt() const
80 return this->_cusp_cnt;
84 RRTNode::cusp_cnt(RRTNode const& p)
86 this->_cusp_cnt = p.cusp_cnt();
87 if (this->_p_is_cusp) {
95 return this->_segment_type;
101 this->_segment_type = st;
105 RRTNode::p_is_cusp(void) const
107 return this->_p_is_cusp;
111 RRTNode::p_is_cusp(bool isit)
113 this->_p_is_cusp = isit;
117 RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
119 if (std::abs(p.sp()) < 1e-3) {
120 // It should not be possible to have two zero speed nodes next
121 // to each other. In practice, this sometimes happens.
122 //assert(std::abs(this->sp()) > 1e-3);
124 if (sgn(p.p()->sp()) != sgn(this->sp())) {
130 return true; // only root has no parent and it is cusp
133 if (std::abs(this->sp()) < 1e-3) {
134 return false; // this is cusp, not the parent
135 } else if (sgn(p.sp()) != sgn(this->sp())) {
144 RRTNode::operator==(RRTNode const& n)
150 RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
152 std::vector<RRTNode*> path;
153 path.reserve(this->_path.size());
154 while (g != nullptr) {
158 std::reverse(path.begin(), path.end());
159 for (unsigned int i = 1; i < path.size(); i++) {
160 path[i]->c(this->cost_build(*path[i - 1], *path[i]));
165 RRTS::recompute_path_cc()
167 this->recompute_cc_for_predecessors_and(&this->_goal);
171 RRTS::min_gamma_eta() const
173 double ns = this->_nodes.size();
174 double gamma = pow(log(ns) / ns, 1.0 / 3.0);
175 return std::min(gamma, this->eta());
179 RRTS::should_continue() const
181 return !this->should_finish();
185 RRTS::join_steered(RRTNode* f)
187 while (this->_steered.size() > 0) {
188 this->store(this->_steered.front());
189 RRTNode* t = &this->_nodes.back();
191 t->c(this->cost_build(*f, *t));
192 this->_steered.erase(this->_steered.begin());
200 RRTNode* f = this->_nn;
201 RRTNode* t = &this->_steered.front();
202 // Require the steer method to return first node equal to nn:
203 assert(std::abs(t->x() - f->x()) < 1e-3
204 && std::abs(t->y() - f->y()) < 1e-3
205 && std::abs(t->h() - f->h()) < 1e-3);
206 this->_steered.erase(this->_steered.begin());
207 t = &this->_steered.front();
208 assert(!(std::abs(t->x() - f->x()) < 1e-3
209 && std::abs(t->y() - f->y()) < 1e-3
210 && std::abs(t->h() - f->h()) < 1e-3));
212 double cost = f->cc() + this->cost_build(*f, *t);
213 for (auto n: this->nv_) {
214 double nc = n->cc() + this->cost_build(*n, *t);
220 // Check if it's possible to drive from *f to *t. If not, then fallback
221 // to *f = _nn. This could be also solved by additional steer from *f to
222 // *t instead of the following code.
223 this->set_bc_pose_to(*f);
224 if (!this->_bc.drivable(*t)) {
228 this->store(this->_steered.front());
229 t = &this->_nodes.back();
231 t->c(this->cost_build(*f, *t));
232 this->_steered.erase(this->_steered.begin());
239 RRTNode *f = &this->_nodes.back();
240 for (auto n: this->_nv) {
241 double fc = f->cc() + this->cost_build(*f, *n);
242 this->set_bc_pose_to(*f);
243 bool drivable = this->_bc.drivable(*n);
244 if (drivable && fc < n->cc()) {
246 n->c(this->cost_build(*f, *n));
252 RRTS::goal_drivable_from(RRTNode const& f)
254 this->set_bc_pose_to(f);
255 return this->_bc.drivable(this->_goal);
259 RRTS::store(RRTNode n)
261 this->_nodes.push_back(n);
265 RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
271 RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
273 return this->cost_build(f, t);
277 RRTS::find_nn(RRTNode const& t)
279 this->_nn = &this->_nodes.front();
280 this->_cost = this->cost_search(*this->_nn, t);
281 for (auto& f: this->_nodes) {
282 if (this->cost_search(f, t) < this->_cost) {
284 this->_cost = this->cost_search(f, t);
290 RRTS::find_nv(RRTNode const& t)
293 this->_cost = this->min_gamma_eta();
294 for (auto& f: this->_nodes) {
295 if (this->cost_search(f, t) < this->_cost) {
296 this->_nv.push_back(&f);
305 RRTNode *g = &this->_goal;
306 if (g->p() == nullptr) {
309 while (g != nullptr && this->_path.size() < 10000) {
312 * There shouldn't be this->_path.size() < 10000 condition.
313 * However, the RRTS::compute_path() called from
314 * RRTExt13::compute_path tends to re-allocate this->_path
315 * infinitely. There's probably node->p() = &node somewhere...
317 this->_path.push_back(g);
320 std::reverse(this->_path.begin(), this->_path.end());
323 RRTS::RRTS() : _goal(0.0, 0.0, 0.0, 0.0), _gen(std::random_device{}())
325 this->_nodes.reserve(4000000);
326 this->_steered.reserve(1000);
327 this->_path.reserve(10000);
328 this->_nv.reserve(1000);
329 this->store(RRTNode()); // root
333 RRTS::set_bc_pose_to(Pose const& p)
335 this->_bc.set_pose_to(p);
339 RRTS::set_bc_to_become(std::string what)
341 this->_bc.become(what);
345 RRTS::goal(void) const
351 RRTS::goal(double x, double y, double b, double e)
353 this->_goal = RRTGoal(x, y, b, e);
357 RRTS::icnt(void) const
363 RRTS::icnt(unsigned int i)
369 RRTS::icnt_max(void) const
371 return this->_icnt_max;
375 RRTS::icnt_max(unsigned int i)
389 return this->_ter.scnt();
393 RRTS::set_init_pose_to(Pose const& p)
395 this->_nodes.front().x(p.x());
396 this->_nodes.front().y(p.y());
397 this->_nodes.front().h(p.h());
403 std::vector<Pose> path;
404 for (auto n: this->_path) {
405 path.push_back(Pose(n->x(), n->y(), n->h()));
411 RRTS::path_cost() const
413 return this->_goal.cc();
417 RRTS::last_path_cost(void) const
419 if (this->_logged_paths.size() == 0) {
422 assert(this->_logged_paths.back().size() > 0);
423 return this->_logged_paths.back().back().cc();
439 RRTS::json(void) const
442 unsigned int i = 0, j = 0;
443 for (auto path: this->_logged_paths) {
446 jvo["paths"][j][i][0] = n.x();
447 jvo["paths"][j][i][1] = n.y();
448 jvo["paths"][j][i][2] = n.h();
449 jvo["paths"][j][i][3] = n.sp();
450 jvo["paths"][j][i][4] = n.st();
451 jvo["paths"][j][i][5] = false;
454 jvo["paths"][j][i - 1][5] = true;
458 // Initial point is part of the first segment.
459 jvo["paths"][j][0][3] = jvo["paths"][j][1][3];
460 jvo["paths"][j][0][4] = jvo["paths"][j][1][4];
461 // Goal point is part of the last segment.
462 jvo["paths"][j][i - 1][3] = jvo["paths"][j][i - 2][3];
463 jvo["paths"][j][i - 1][4] = jvo["paths"][j][i - 2][4];
465 jvo["costs"][j] = path.back().cc();
469 for (auto n: this->_path) {
470 jvo["paths"][j][i][0] = n->x();
471 jvo["paths"][j][i][1] = n->y();
472 jvo["paths"][j][i][2] = n->h();
473 jvo["paths"][j][i][3] = n->sp();
474 jvo["paths"][j][i][4] = n->st();
475 jvo["paths"][j][i][5] = n->p_is_cusp();
476 jvo["path"][i][0] = n->x();
477 jvo["path"][i][1] = n->y();
478 jvo["path"][i][2] = n->h();
479 jvo["path"][i][3] = n->sp();
480 jvo["path"][i][4] = n->st();
481 jvo["path"][i][5] = false;
482 if (n->p_is_cusp()) {
484 jvo["path"][i - 1][5] = true;
488 // Initial point is part of the first segment.
489 jvo["path"][0][3] = jvo["path"][1][3];
490 jvo["path"][0][4] = jvo["path"][1][4];
491 // Goal point is part of the last segment.
492 jvo["path"][i - 1][3] = jvo["path"][i - 2][3];
493 jvo["path"][i - 1][4] = jvo["path"][i - 2][4];
495 jvo["costs"][j] = this->_path.back()->cc();
496 jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
497 jvo["cost"] = this->path_cost();
498 jvo["time"] = this->scnt();
503 RRTS::json(Json::Value jvi)
505 assert(jvi["init"] != Json::nullValue);
506 assert(jvi["goal"] != Json::nullValue);
507 this->set_init_pose_to(Pose(
508 jvi["init"][0].asDouble(),
509 jvi["init"][1].asDouble(),
510 jvi["init"][2].asDouble()));
511 if (jvi["goal"].size() == 4) {
513 jvi["goal"][0].asDouble(),
514 jvi["goal"][1].asDouble(),
515 jvi["goal"][2].asDouble(),
516 jvi["goal"][3].asDouble());
519 jvi["goal"][0].asDouble(),
520 jvi["goal"][1].asDouble(),
521 jvi["goal"][2].asDouble(),
522 jvi["goal"][2].asDouble());
529 if (this->icnt() == 0) {
532 auto rs = this->sample();
533 #if 1 // anytime RRTs
535 double d1 = this->cost_search(this->_nodes.front(), rs);
536 double d2 = this->cost_search(rs, this->_goal);
537 if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
538 auto& last_path = this->_logged_paths.back();
539 rs = last_path[rand() % last_path.size()];
544 this->steer(*this->_nn, rs);
545 if (this->collide_steered()) {
546 return this->should_continue();
549 this->find_nv(this->_steered.front());
551 if (!this->connect()) {
552 return this->should_continue();
557 unsigned int ss = this->_steered.size();
558 this->join_steered(&this->_nodes.back());
559 RRTNode* just_added = &this->_nodes.back();
561 while (ss > 0 && just_added != nullptr) {
562 this->steer(*just_added, this->_goal);
563 if (this->collide_steered()) {
565 just_added = just_added->p();
568 // The first of steered is the same as just_added.
569 this->_steered.erase(this->_steered.begin());
570 this->join_steered(just_added);
571 bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
572 bool gd = this->goal_drivable_from(this->_nodes.back());
574 double nc = this->cost_build(
577 double ncc = this->_nodes.back().cc() + nc;
578 if (this->_goal.p() == nullptr
579 || ncc < this->_goal.cc()) {
580 this->_goal.p(this->_nodes.back(), true);
586 just_added = just_added->p();
589 this->compute_path();
591 this->_time = this->scnt();
592 this->icnt(this->icnt() + 1);
593 return this->should_continue();
599 if (this->path_cost() != 0.0
600 && this->path_cost() < this->last_path_cost()) {
601 this->_logged_paths.push_back(std::vector<RRTNode>());
602 auto& last_path = this->_logged_paths.back();
603 last_path.reserve(this->_path.size());
604 RRTNode* p = nullptr;
605 for (auto n: this->_path) {
606 last_path.push_back(*n);
608 last_path.back().p(*p);
610 p = &last_path.back();
612 // Test that last path cost matches.
613 auto last_path_cost = last_path.back().cc();
614 for (unsigned int i = 1; i < last_path.size(); i++) {
615 last_path[i].c(this->cost_build(
619 assert(last_path_cost == last_path.back().cc());
621 this->_goal = RRTGoal(
627 this->_steered.clear();
628 this->_nodes.erase(this->_nodes.begin() + 1, this->_nodes.end());
631 this->_bc = BicycleCar();