2 This file is part of I am car.
4 I am car is free software: you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation, either version 3 of the License, or
7 (at your option) any later version.
9 I am car is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
14 You should have received a copy of the GNU General Public License
15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
23 #include "rrtplanner.h"
26 #define CATI(a, b) a ## b
27 #define CAT(a, b) CATI(a, b)
28 #define KUWATA2008_CCOST CAT(c, CO)
29 #define KUWATA2008_DCOST CO
31 LaValle1998::LaValle1998(RRTNode *init, RRTNode *goal):
34 srand(static_cast<unsigned>(time(0)));
37 bool LaValle1998::next()
41 if (this->samples().size() == 0)
48 this->samples().push_back(rs);
49 RRTNode *nn = this->nn(rs);
52 for (auto ns: this->steer(nn, rs)) {
56 this->nodes().push_back(ns);
58 pn->add_child(ns, this->cost(pn, ns));
59 if (this->collide(pn, ns)) {
60 pn->children().pop_back();
62 this->iy_[this->YI(ns)].pop_back();
67 if (this->goal_found(pn, CO)) {
68 this->tlog(this->findt());
74 return this->goal_found();
77 Kuwata2008::Kuwata2008(RRTNode *init, RRTNode *goal):
80 srand(static_cast<unsigned>(time(0)));
83 bool Kuwata2008::next()
86 if (this->samples().size() == 0) {
91 this->samples().push_back(rs);
92 float heur = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
93 if (this->goal_found()) {
95 {}//this->cost = &KUWATA2008_CCOST;
97 {}//this->cost = &KUWATA2008_DCOST;
100 {}//this->cost = &KUWATA2008_CCOST;
102 {}//this->cost = &KUWATA2008_DCOST;
104 RRTNode *nn = this->nn(rs);
106 std::vector<RRTNode *> newly_added;
108 for (auto ns: this->steer(nn, rs)) {
112 this->nodes().push_back(ns);
114 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
115 if (this->collide(pn, ns)) {
116 pn->children().pop_back();
118 this->iy_[this->YI(ns)].pop_back();
123 newly_added.push_back(pn);
124 if (this->goal_found(pn, &KUWATA2008_DCOST)) {
125 this->tlog(this->findt());
131 if (this->samples().size() <= 1)
132 return this->goal_found();
133 for (auto na: newly_added) {
136 for (auto ns: this->steer(na, this->goal())) {
140 this->nodes().push_back(ns);
142 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
143 if (this->collide(pn, ns)) {
144 pn->children().pop_back();
146 this->iy_[this->YI(ns)].pop_back();
151 if (this->goal_found(pn,
152 &KUWATA2008_DCOST)) {
153 this->tlog(this->findt());
160 return this->goal_found();
163 Karaman2011::Karaman2011()
165 srand(static_cast<unsigned>(time(0)));
168 Karaman2011::Karaman2011(RRTNode *init, RRTNode *goal)
169 : RRTBase(init, goal)
171 srand(static_cast<unsigned>(time(0)));
174 bool Karaman2011::next()
178 if (this->samples().size() == 0)
185 this->samples().push_back(rs);
186 RRTNode *nn = this->nn(rs);
188 std::vector<RRTNode *> nvs;
191 for (auto ns: this->steer(nn, rs)) {
194 } else if (IS_NEAR(nn, ns)) {
201 this->nodes().size()),
210 this->nodes().push_back(ns);
213 if (!this->connect(pn, ns, nvs)) {
214 this->iy_[this->YI(ns)].pop_back();
218 this->rewire(nvs, ns);
220 if (this->goal_found(pn, CO)) {
221 this->tlog(this->findt());
227 return this->goal_found();
230 bool Karaman2011::connect(
233 std::vector<RRTNode *> nvs)
235 RRTNode *op; // old parent
236 float od; // old direct cost
237 float oc; // old cumulative cost
238 bool connected = false;
239 pn->add_child(ns, this->cost(pn, ns));
240 if (this->collide(pn, ns)) {
241 pn->children().pop_back();
248 if (!connected || (nv->ccost() + this->cost(nv, ns) <
253 nv->add_child(ns, this->cost(nv, ns));
254 if (this->collide(nv, ns)) {
255 nv->children().pop_back();
262 } else if (connected) {
263 op->children().pop_back();
273 bool Karaman2011::rewire(std::vector<RRTNode *> nvs, RRTNode *ns)
275 RRTNode *op; // old parent
276 float od; // old direct cost
277 float oc; // old cumulative cost
279 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
283 ns->add_child(nv, this->cost(ns, nv));
284 if (this->collide(ns, nv)) {
285 ns->children().pop_back();
297 T1::T1(RRTNode *init, RRTNode *goal):
300 srand(static_cast<unsigned>(time(0)));
306 if (this->samples().size() == 0)
310 this->samples().push_back(rs);
311 RRTNode *nn = this->nn(rs);
313 std::vector<RRTNode *> nvs;
315 RRTNode *op; // old parent
316 float od; // old direct cost
317 float oc; // old cumulative cost
318 std::vector<RRTNode *> steered = this->steer(nn, rs);
319 // RRT* for first node
320 RRTNode *ns = steered[0];
322 nvs = this->nv(ns, MIN(
323 GAMMA_RRTSTAR(this->nodes().size()),
325 this->nodes().push_back(ns);
328 pn->add_child(ns, this->cost(pn, ns));
329 if (this->collide(pn, ns)) {
330 pn->children().pop_back();
336 if (!connected || (nv->ccost() + this->cost(nv, ns) <
341 nv->add_child(ns, this->cost(nv, ns));
342 if (this->collide(nv, ns)) {
343 nv->children().pop_back();
347 } else if (connected) {
348 op->children().pop_back();
358 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
362 ns->add_child(nv, this->cost(ns, nv));
363 if (this->collide(ns, nv)) {
364 ns->children().pop_back();
374 if (this->goal_found(pn, CO)) {
375 this->tlog(this->findt());
379 for (i = 1; i < steered.size(); i++) {
381 this->nodes().push_back(ns);
383 pn->add_child(ns, this->cost(pn, ns));
384 if (this->collide(pn, ns)) {
385 pn->children().pop_back();
389 if (this->goal_found(pn, CO)) {
390 this->tlog(this->findt());
394 return this->goal_found();
400 if (this->firsts().size() > 0) {
401 rs = this->firsts().front();
402 this->firsts().pop();
406 this->samples().push_back(rs);
407 RRTNode *nn = this->nn(rs);
411 std::vector<RRTNode *> nvs;
412 std::vector<RRTNode *> newly_added;
415 for (auto ns: this->steer(nn, rs)) {
419 } else if (IS_NEAR(pn, ns)) {
422 if (sgn(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
430 this->nodes().size()),
432 this->nodes().push_back(ns);
435 if (!this->connect(pn, ns, nvs)) {
436 this->iy_[this->YI(ns)].pop_back();
437 this->nodes().pop_back();
443 this->rewire(nvs, ns);
445 newly_added.push_back(pn);
446 if (this->goal_found(pn, CO)) {
448 this->tlog(this->findt());
454 if (this->samples().size() <= 1)
455 return this->goal_found();
457 for (auto na: newly_added) {
461 for (auto ns: this->steer(na, this->goal())) {
465 } else if (IS_NEAR(pn, ns)) {
468 if (sgn(pn->s()) != sgn(ns->s()))
472 this->nodes().push_back(ns);
474 pn->add_child(ns, this->cost(pn, ns));
475 if (this->collide(pn, ns)) {
476 pn->children().pop_back();
478 this->iy_[this->YI(ns)].pop_back();
479 this->nodes().pop_back();
486 if (this->goal_found(pn, CO)) {
488 this->tlog(this->findt());
496 for (auto na: newly_added) {
497 for (auto go: this->goals()) {
501 for (auto ns: this->steer(na, go)) {
505 } else if (IS_NEAR(pn, ns)) {
508 if (sgn(pn->s()) != sgn(ns->s()))
512 this->nodes().push_back(ns);
514 pn->add_child(ns, this->cost(pn, ns));
515 if (this->collide(pn, ns)) {
516 pn->children().pop_back();
518 this->iy_[this->YI(ns)].pop_back();
519 this->nodes().pop_back();
526 if (this->goal_found(pn, go)) {
527 this->tlog(this->findt());
534 return this->goal_found();
537 float T2::goal_cost()
539 std::vector<RRTNode *> nvs;
540 nvs = this->nv(this->goal(), 0.2);
542 if (std::abs(this->goal()->h() - nv->h()) >=
543 this->GOAL_FOUND_ANGLE)
545 if (nv->ccost() + this->cost(nv, this->goal()) >=
546 this->goal()->ccost())
548 RRTNode *op; // old parent
549 float oc; // old cumulative cost
550 float od; // old direct cost
551 op = this->goal()->parent();
552 oc = this->goal()->ccost();
553 od = this->goal()->dcost();
554 nv->add_child(this->goal(),
555 this->cost(nv, this->goal()));
556 if (this->collide(nv, this->goal())) {
557 nv->children().pop_back();
558 this->goal()->parent(op);
559 this->goal()->ccost(oc);
560 this->goal()->dcost(od);
562 op->rem_child(this->goal());
565 return this->goal()->ccost();
570 for (auto n: this->p_root_.nodes())
571 if (n != this->p_root_.root() && n != this->p_root_.goal())
573 for (auto n: this->p_root_.dnodes())
574 if (n != this->p_root_.root() && n != this->p_root_.goal())
576 for (auto s: this->p_root_.samples())
577 if (s != this->p_root_.goal())
579 for (auto edges: this->p_root_.rlog())
583 for (auto n: this->p_goal_.nodes())
584 if (n != this->p_goal_.root() && n != this->p_goal_.goal())
586 for (auto n: this->p_goal_.dnodes())
587 if (n != this->p_goal_.root() && n != this->p_goal_.goal())
589 for (auto s: this->p_goal_.samples())
590 if (s != this->p_goal_.goal())
592 for (auto edges: this->p_goal_.rlog())
596 for (auto n: this->nodes())
597 if (n != this->root())
599 for (auto n: this->dnodes())
600 if (n != this->root() && n != this->goal())
602 for (auto s: this->samples())
603 if (s != this->goal())
605 for (auto edges: this->rlog())
615 srand(static_cast<unsigned>(time(0)));
618 T3::T3(RRTNode *init, RRTNode *goal):
623 srand(static_cast<unsigned>(time(0)));
628 RRTNode *ron = nullptr;
629 RRTNode *gon = nullptr;
631 ret = this->p_root_.next();
632 ret |= this->p_goal_.next();
633 if (this->overlaptrees(&ron, &gon)) {
634 if (this->connecttrees(ron, gon))
635 this->goal_found(true);
636 this->tlog(this->findt());
642 bool T3::link_obstacles(
643 std::vector<CircleObstacle> *cobstacles,
644 std::vector<SegmentObstacle> *sobstacles)
647 ret = RRTBase::link_obstacles(cobstacles, sobstacles);
648 ret &= this->p_root_.link_obstacles(cobstacles, sobstacles);
649 ret &= this->p_goal_.link_obstacles(cobstacles, sobstacles);
653 bool T3::connecttrees(RRTNode *rn, RRTNode *gn)
655 while (gn != this->goal()) {
656 this->p_root_.nodes().push_back(new RRTNode(
661 this->p_root_.nodes().back(),
664 this->p_root_.nodes().back()));
665 rn = this->p_root_.nodes().back();
668 rn->add_child(this->goal(), this->p_root_.cost(rn, this->goal()));
672 bool T3::overlaptrees(RRTNode **ron, RRTNode **gon)
674 for (auto rn: this->p_root_.nodes()) {
675 if (rn->parent() == nullptr)
677 for (auto gn: this->p_goal_.nodes()) {
678 if (gn->parent() == nullptr)
680 if (IS_NEAR(rn, gn)) {
690 Klemm2015::Klemm2015(RRTNode *init, RRTNode *goal):
691 Karaman2011(init, goal),
695 srand(static_cast<unsigned>(time(0)));
696 this->root()->tree('R');
697 this->goal()->tree('G');
698 this->add_iy(this->goal());
701 bool Klemm2015::next()
703 RRTNode *xn = nullptr;
707 if (this->samples().size() == 0)
714 this->samples().push_back(rs);
715 //std::cerr << "next" << std::endl;
716 if (this->extendstar1(rs, &xn) != 2) {
718 // std::cerr << "- xn: " << xn->x() << ", " << xn->y();
719 // std::cerr << std::endl;
721 // std::cerr << "- xn: nullptr" << std::endl;
724 ret = this->connectstar(xn);
729 this->tlog(this->findt());
732 return this->goal_found();
735 int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
737 int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
738 char tree = this->root()->tree();
739 //std::cerr << "extend*1" << std::endl;
740 //std::cerr << "- tree is " << tree << std::endl;
741 //std::cerr << " - rs: " << rs->x() << ", " << rs->y() << std::endl;
743 // std::cerr << " - xn: " << (*xn)->x() << ", " << (*xn)->y();
744 // std::cerr << std::endl;
746 //for (int i = 0; i < IYSIZE; i++) {
747 // if (this->iy_[i].size() > 0) {
748 // RRTNode *tmpn = this->iy_[i].back();
749 // float tmpd = EDIST(tmpn, this->goal());
751 // std::cerr << i << ": " << tmpn->x();
752 // std::cerr << ", " << tmpn->y();
753 // std::cerr << ", " << tmpn->tree();
754 // std::cerr << " (" << tmpd << ")";
756 // if (tmpn == this->root())
757 // std::cerr << " root";
758 // if (tmpn == this->goal())
759 // std::cerr << " goal";
760 // std::cerr << std::endl;
763 RRTNode *nn = this->nn(rs);
764 //std::cerr << " - nn: " << nn->x() << ", " << nn->y() << std::endl;
765 std::vector<RRTNode *> nvs;
766 std::vector<RRTNode *> steered = this->steer(nn, rs);
767 RRTNode *ns = steered[1];
773 this->nodes().size()),
775 this->nodes().push_back(ns);
778 if (!this->connect(nn, ns, nvs)) {
779 this->iy_[this->YI(ns)].pop_back();
783 this->rewire(nvs, ns);
785 for (auto n: steered) {
790 //std::cerr << " - xn: " << (*xn)->x() << ", " << (*xn)->y();
791 //std::cerr << std::endl;
795 int Klemm2015::extendstarC(RRTNode *rs)
797 int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
798 char tree = this->root()->tree();
799 //std::cerr << "extend*C" << std::endl;
800 //std::cerr << "- tree is " << tree << std::endl;
801 //std::cerr << " - rs: " << rs->x() << ", " << rs->y() << std::endl;
802 RRTNode *nn = this->nn(rs);
804 std::vector<RRTNode *> nvs;
806 for (auto ns: this->steer(nn, rs)) {
814 this->nodes().size()),
816 this->nodes().push_back(ns);
819 if (!this->connect(pn, ns, nvs)) {
820 this->iy_[this->YI(ns)].pop_back();
825 this->rewire(nvs, ns);
827 if (IS_NEAR(pn, rs)) { // GOAL FOUND !
829 if (this->orig_root_ == this->root()) { // rs is in G tree
830 // add all rs parents to pn
832 } else { // rs is in R tree
837 while (tmp != this->goal()) {
838 this->nodes().push_back(new RRTNode(
842 this->nodes().back()->s(tmp->s());
843 this->nodes().back()->tree('R');
845 this->nodes().back(),
846 this->cost(pn, this->nodes().back()));
847 pn = this->nodes().back();
850 pn->add_child(tmp, this->cost(pn, tmp)); // add goal()
860 int Klemm2015::connectstar(RRTNode *x)
862 int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
863 //std::cerr << "connect* (start)" << std::endl;
864 ret = this->extendstarC(x);
865 //std::cerr << "connect* (end)" << std::endl;
869 void Klemm2015::swap()
873 this->root(this->goal());