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/>.
26 #include "rrtplanner.h"
29 #define CATI(a, b) a ## b
30 #define CAT(a, b) CATI(a, b)
31 #define KUWATA2008_CCOST CAT(c, CO)
32 #define KUWATA2008_DCOST CO
34 LaValle1998::LaValle1998(RRTNode *init, RRTNode *goal):
37 srand(static_cast<unsigned>(time(0)));
40 bool LaValle1998::next()
44 if (this->samples().size() == 0)
51 this->samples().push_back(rs);
52 RRTNode *nn = this->nn(rs);
55 for (auto ns: this->steer(nn, rs)) {
59 this->nodes().push_back(ns);
61 pn->add_child(ns, this->cost(pn, ns));
62 if (this->collide(pn, ns)) {
63 pn->children().pop_back();
65 this->iy_[IYI(ns->y())].pop_back();
70 if (this->goal_found(pn, CO)) {
71 this->tlog(this->findt());
77 return this->goal_found();
80 Kuwata2008::Kuwata2008(RRTNode *init, RRTNode *goal):
83 srand(static_cast<unsigned>(time(0)));
86 bool Kuwata2008::next()
89 if (this->samples().size() == 0) {
94 this->samples().push_back(rs);
95 float heur = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
96 if (this->goal_found()) {
98 {}//this->cost = &KUWATA2008_CCOST;
100 {}//this->cost = &KUWATA2008_DCOST;
103 {}//this->cost = &KUWATA2008_CCOST;
105 {}//this->cost = &KUWATA2008_DCOST;
107 RRTNode *nn = this->nn(rs);
109 std::vector<RRTNode *> newly_added;
111 for (auto ns: this->steer(nn, rs)) {
115 this->nodes().push_back(ns);
117 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
118 if (this->collide(pn, ns)) {
119 pn->children().pop_back();
121 this->iy_[IYI(ns->y())].pop_back();
126 newly_added.push_back(pn);
127 if (this->goal_found(pn, &KUWATA2008_DCOST)) {
128 this->tlog(this->findt());
134 if (this->samples().size() <= 1)
135 return this->goal_found();
136 for (auto na: newly_added) {
139 for (auto ns: this->steer(na, this->goal())) {
143 this->nodes().push_back(ns);
145 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
146 if (this->collide(pn, ns)) {
147 pn->children().pop_back();
149 this->iy_[IYI(ns->y())].pop_back();
154 if (this->goal_found(pn,
155 &KUWATA2008_DCOST)) {
156 this->tlog(this->findt());
163 return this->goal_found();
166 Karaman2011::Karaman2011()
168 srand(static_cast<unsigned>(time(0)));
171 Karaman2011::Karaman2011(RRTNode *init, RRTNode *goal)
172 : RRTBase(init, goal)
174 srand(static_cast<unsigned>(time(0)));
177 bool Karaman2011::next()
181 if (this->samples().size() == 0)
188 this->samples().push_back(rs);
189 RRTNode *nn = this->nn(rs);
191 std::vector<RRTNode *> nvs;
194 for (auto ns: this->steer(nn, rs)) {
197 } else if (IS_NEAR(nn, ns)) {
204 this->nodes().size()),
213 this->nodes().push_back(ns);
216 if (!this->connect(pn, ns, nvs)) {
217 this->iy_[IYI(ns->y())].pop_back();
221 this->rewire(nvs, ns);
223 if (this->goal_found(pn, CO)) {
224 this->tlog(this->findt());
230 return this->goal_found();
233 bool Karaman2011::connect(
236 std::vector<RRTNode *> nvs)
238 RRTNode *op; // old parent
239 float od; // old direct cost
240 float oc; // old cumulative cost
241 bool connected = false;
242 pn->add_child(ns, this->cost(pn, ns));
243 if (this->collide(pn, ns)) {
244 pn->children().pop_back();
251 if (!connected || (nv->ccost() + this->cost(nv, ns) <
256 nv->add_child(ns, this->cost(nv, ns));
257 if (this->collide(nv, ns)) {
258 nv->children().pop_back();
265 } else if (connected) {
266 op->children().pop_back();
276 bool Karaman2011::rewire(std::vector<RRTNode *> nvs, RRTNode *ns)
278 RRTNode *op; // old parent
279 float od; // old direct cost
280 float oc; // old cumulative cost
282 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
286 ns->add_child(nv, this->cost(ns, nv));
287 if (this->collide(ns, nv)) {
288 ns->children().pop_back();
300 T1::T1(RRTNode *init, RRTNode *goal):
303 srand(static_cast<unsigned>(time(0)));
309 if (this->samples().size() == 0)
313 this->samples().push_back(rs);
314 RRTNode *nn = this->nn(rs);
316 std::vector<RRTNode *> nvs;
318 RRTNode *op; // old parent
319 float od; // old direct cost
320 float oc; // old cumulative cost
321 std::vector<RRTNode *> steered = this->steer(nn, rs);
322 // RRT* for first node
323 RRTNode *ns = steered[0];
325 nvs = this->nv(ns, MIN(
326 GAMMA_RRTSTAR(this->nodes().size()),
328 this->nodes().push_back(ns);
331 pn->add_child(ns, this->cost(pn, ns));
332 if (this->collide(pn, ns)) {
333 pn->children().pop_back();
339 if (!connected || (nv->ccost() + this->cost(nv, ns) <
344 nv->add_child(ns, this->cost(nv, ns));
345 if (this->collide(nv, ns)) {
346 nv->children().pop_back();
350 } else if (connected) {
351 op->children().pop_back();
361 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
365 ns->add_child(nv, this->cost(ns, nv));
366 if (this->collide(ns, nv)) {
367 ns->children().pop_back();
377 if (this->goal_found(pn, CO)) {
378 this->tlog(this->findt());
382 for (i = 1; i < steered.size(); i++) {
384 this->nodes().push_back(ns);
386 pn->add_child(ns, this->cost(pn, ns));
387 if (this->collide(pn, ns)) {
388 pn->children().pop_back();
392 if (this->goal_found(pn, CO)) {
393 this->tlog(this->findt());
397 return this->goal_found();
403 if (this->firsts().size() > 0) {
404 rs = this->firsts().front();
405 this->firsts().pop();
409 this->samples().push_back(rs);
410 RRTNode *nn = this->nn(rs);
414 std::vector<RRTNode *> nvs;
415 std::vector<RRTNode *> newly_added;
418 for (auto ns: this->steer(nn, rs)) {
422 } else if (IS_NEAR(pn, ns)) {
425 if (sgn(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
433 this->nodes().size()),
435 this->nodes().push_back(ns);
438 if (!this->connect(pn, ns, nvs)) {
439 this->iy_[IYI(ns->y())].pop_back();
440 this->nodes().pop_back();
446 this->rewire(nvs, ns);
448 newly_added.push_back(pn);
449 if (this->goal_found(pn, CO)) {
451 this->tlog(this->findt());
459 for (auto ns: this->steer(pn, rs, 0.01)) {
463 } else if (IS_NEAR(pn, ns)) {
466 if (sgn(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
474 this->nodes().size()),
476 this->nodes().push_back(ns);
479 if (!this->connect(pn, ns, nvs)) {
480 this->iy_[IYI(ns->y())].pop_back();
481 this->nodes().pop_back();
487 this->rewire(nvs, ns);
489 newly_added.push_back(pn);
490 if (this->goal_found(pn, CO)) {
492 this->tlog(this->findt());
498 if (this->samples().size() <= 1)
499 return this->goal_found();
501 for (auto na: newly_added) {
505 for (auto ns: this->steer(na, this->goal())) {
509 } else if (IS_NEAR(pn, ns)) {
512 if (sgn(pn->s()) != sgn(ns->s()))
516 this->nodes().push_back(ns);
518 pn->add_child(ns, this->cost(pn, ns));
519 if (this->collide(pn, ns)) {
520 pn->children().pop_back();
522 this->iy_[IYI(ns->y())].pop_back();
523 this->nodes().pop_back();
530 if (this->goal_found(pn, CO)) {
532 this->tlog(this->findt());
540 for (auto ns: this->steer(pn, this->goal(), 0.01)) {
544 } else if (IS_NEAR(pn, ns)) {
547 if (sgn(pn->s()) != sgn(ns->s()))
551 this->nodes().push_back(ns);
553 pn->add_child(ns, this->cost(pn, ns));
554 if (this->collide(pn, ns)) {
555 pn->children().pop_back();
557 this->iy_[IYI(ns->y())].pop_back();
558 this->nodes().pop_back();
565 if (this->goal_found(pn, CO)) {
567 this->tlog(this->findt());
575 for (auto na: newly_added) {
576 for (auto go: this->goals()) {
580 for (auto ns: this->steer(na, go)) {
584 } else if (IS_NEAR(pn, ns)) {
587 if (sgn(pn->s()) != sgn(ns->s()))
591 this->nodes().push_back(ns);
593 pn->add_child(ns, this->cost(pn, ns));
594 if (this->collide(pn, ns)) {
595 pn->children().pop_back();
597 this->iy_[IYI(ns->y())].pop_back();
598 this->nodes().pop_back();
605 if (this->goal_found(pn, go)) {
606 this->tlog(this->findt());
613 return this->goal_found();
616 float T2::goal_cost()
618 std::vector<RRTNode *> nvs;
619 nvs = this->nv(this->goal(), 0.2);
621 if (std::abs(this->goal()->h() - nv->h()) >=
622 this->GOAL_FOUND_ANGLE)
624 if (nv->ccost() + this->cost(nv, this->goal()) >=
625 this->goal()->ccost())
627 RRTNode *op; // old parent
628 float oc; // old cumulative cost
629 float od; // old direct cost
630 op = this->goal()->parent();
631 oc = this->goal()->ccost();
632 od = this->goal()->dcost();
633 nv->add_child(this->goal(),
634 this->cost(nv, this->goal()));
635 if (this->collide(nv, this->goal())) {
636 nv->children().pop_back();
637 this->goal()->parent(op);
638 this->goal()->ccost(oc);
639 this->goal()->dcost(od);
641 op->rem_child(this->goal());
644 return this->goal()->ccost();
649 for (auto n: this->p_root_.nodes())
650 if (n != this->p_root_.root() && n != this->p_root_.goal())
652 for (auto n: this->p_root_.dnodes())
653 if (n != this->p_root_.root() && n != this->p_root_.goal())
655 for (auto s: this->p_root_.samples())
656 if (s != this->p_root_.goal())
658 for (auto edges: this->p_root_.rlog())
662 for (auto n: this->p_goal_.nodes())
663 if (n != this->p_goal_.root() && n != this->p_goal_.goal())
665 for (auto n: this->p_goal_.dnodes())
666 if (n != this->p_goal_.root() && n != this->p_goal_.goal())
668 for (auto s: this->p_goal_.samples())
669 if (s != this->p_goal_.goal())
671 for (auto edges: this->p_goal_.rlog())
675 for (auto n: this->nodes())
676 if (n != this->root())
678 for (auto n: this->dnodes())
679 if (n != this->root() && n != this->goal())
681 for (auto s: this->samples())
682 if (s != this->goal())
684 for (auto edges: this->rlog())
694 srand(static_cast<unsigned>(time(0)));
697 T3::T3(RRTNode *init, RRTNode *goal):
702 srand(static_cast<unsigned>(time(0)));
707 RRTNode *ron = nullptr;
708 RRTNode *gon = nullptr;
710 ret = this->p_root_.next();
711 ret |= this->p_goal_.next();
712 if (this->overlaptrees(&ron, &gon)) {
713 if (this->connecttrees(ron, gon))
714 this->goal_found(true);
715 this->tlog(this->findt());
721 bool T3::link_obstacles(
722 std::vector<CircleObstacle> *cobstacles,
723 std::vector<SegmentObstacle> *sobstacles)
726 ret = RRTBase::link_obstacles(cobstacles, sobstacles);
727 ret &= this->p_root_.link_obstacles(cobstacles, sobstacles);
728 ret &= this->p_goal_.link_obstacles(cobstacles, sobstacles);
732 bool T3::connecttrees(RRTNode *rn, RRTNode *gn)
734 while (gn != this->goal()) {
735 this->p_root_.nodes().push_back(new RRTNode(
740 this->p_root_.nodes().back(),
743 this->p_root_.nodes().back()));
744 rn = this->p_root_.nodes().back();
747 rn->add_child(this->goal(), this->p_root_.cost(rn, this->goal()));
751 bool T3::overlaptrees(RRTNode **ron, RRTNode **gon)
753 for (auto rn: this->p_root_.nodes()) {
754 if (rn->parent() == nullptr)
756 for (auto gn: this->p_goal_.nodes()) {
757 if (gn->parent() == nullptr)
759 if (IS_NEAR(rn, gn)) {
769 Klemm2015::Klemm2015(RRTNode *init, RRTNode *goal):
770 Karaman2011(init, goal),
774 srand(static_cast<unsigned>(time(0)));
775 this->root()->tree('R');
776 this->goal()->tree('G');
777 this->add_iy(this->goal());
780 bool Klemm2015::next()
782 RRTNode *xn = nullptr;
786 if (this->samples().size() == 0)
793 this->samples().push_back(rs);
794 //std::cerr << "next" << std::endl;
795 if (this->extendstar1(rs, &xn) != 2) {
797 // std::cerr << "- xn: " << xn->x() << ", " << xn->y();
798 // std::cerr << std::endl;
800 // std::cerr << "- xn: nullptr" << std::endl;
803 ret = this->connectstar(xn);
808 this->tlog(this->findt());
811 return this->goal_found();
814 int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
816 int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
817 char tree = this->root()->tree();
818 //std::cerr << "extend*1" << std::endl;
819 //std::cerr << "- tree is " << tree << std::endl;
820 //std::cerr << " - rs: " << rs->x() << ", " << rs->y() << std::endl;
822 // std::cerr << " - xn: " << (*xn)->x() << ", " << (*xn)->y();
823 // std::cerr << std::endl;
825 //for (int i = 0; i < IYSIZE; i++) {
826 // if (this->iy_[i].size() > 0) {
827 // RRTNode *tmpn = this->iy_[i].back();
828 // float tmpd = EDIST(tmpn, this->goal());
830 // std::cerr << i << ": " << tmpn->x();
831 // std::cerr << ", " << tmpn->y();
832 // std::cerr << ", " << tmpn->tree();
833 // std::cerr << " (" << tmpd << ")";
835 // if (tmpn == this->root())
836 // std::cerr << " root";
837 // if (tmpn == this->goal())
838 // std::cerr << " goal";
839 // std::cerr << std::endl;
842 RRTNode *nn = this->nn(rs);
843 //std::cerr << " - nn: " << nn->x() << ", " << nn->y() << std::endl;
844 std::vector<RRTNode *> nvs;
845 std::vector<RRTNode *> steered = this->steer(nn, rs);
846 RRTNode *ns = steered[1];
852 this->nodes().size()),
854 this->nodes().push_back(ns);
857 if (!this->connect(nn, ns, nvs)) {
858 this->iy_[IYI(ns->y())].pop_back();
862 this->rewire(nvs, ns);
864 for (auto n: steered) {
869 //std::cerr << " - xn: " << (*xn)->x() << ", " << (*xn)->y();
870 //std::cerr << std::endl;
874 int Klemm2015::extendstarC(RRTNode *rs)
876 int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
877 char tree = this->root()->tree();
878 //std::cerr << "extend*C" << std::endl;
879 //std::cerr << "- tree is " << tree << std::endl;
880 //std::cerr << " - rs: " << rs->x() << ", " << rs->y() << std::endl;
881 RRTNode *nn = this->nn(rs);
883 std::vector<RRTNode *> nvs;
885 for (auto ns: this->steer(nn, rs)) {
893 this->nodes().size()),
895 this->nodes().push_back(ns);
898 if (!this->connect(pn, ns, nvs)) {
899 this->iy_[IYI(ns->y())].pop_back();
904 this->rewire(nvs, ns);
906 if (IS_NEAR(pn, rs)) { // GOAL FOUND !
908 if (this->orig_root_ == this->root()) { // rs is in G tree
909 // add all rs parents to pn
911 } else { // rs is in R tree
916 while (tmp != this->goal()) {
917 this->nodes().push_back(new RRTNode(
921 this->nodes().back()->s(tmp->s());
922 this->nodes().back()->tree('R');
924 this->nodes().back(),
925 this->cost(pn, this->nodes().back()));
926 pn = this->nodes().back();
929 pn->add_child(tmp, this->cost(pn, tmp)); // add goal()
939 int Klemm2015::connectstar(RRTNode *x)
941 int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
942 //std::cerr << "connect* (start)" << std::endl;
943 ret = this->extendstarC(x);
944 //std::cerr << "connect* (end)" << std::endl;
948 void Klemm2015::swap()
952 this->root(this->goal());