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/>.
27 #include "rrtplanner.h"
30 #define CATI(a, b) a ## b
31 #define CAT(a, b) CATI(a, b)
32 #define KUWATA2008_CCOST CAT(c, CO)
33 #define KUWATA2008_DCOST CO
35 LaValle1998::LaValle1998(RRTNode *init, RRTNode *goal):
42 srand(static_cast<unsigned>(time(0)));
45 bool LaValle1998::next()
49 if (this->samples().size() == 0)
56 this->samples().push_back(rs);
58 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
60 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
64 for (auto ns: this->steer(nn, rs)) {
68 this->nodes().push_back(ns);
70 pn->add_child(ns, this->cost(pn, ns));
71 if (this->collide(pn, ns)) {
72 pn->children().pop_back();
74 this->iy_[IYI(ns->y())].pop_back();
79 if (this->goal_found(pn, this->cost)) {
80 this->tlog(this->findt());
86 return this->goal_found();
89 Kuwata2008::Kuwata2008(RRTNode *init, RRTNode *goal):
94 cost(KUWATA2008_DCOST)
96 srand(static_cast<unsigned>(time(0)));
99 bool Kuwata2008::next()
102 if (this->samples().size() == 0) {
107 this->samples().push_back(rs);
108 float heur = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
109 if (this->goal_found()) {
111 this->cost = &KUWATA2008_CCOST;
113 this->cost = &KUWATA2008_DCOST;
116 this->cost = &KUWATA2008_CCOST;
118 this->cost = &KUWATA2008_DCOST;
121 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
123 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
126 std::vector<RRTNode *> newly_added;
128 for (auto ns: this->steer(nn, rs)) {
132 this->nodes().push_back(ns);
134 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
135 if (this->collide(pn, ns)) {
136 pn->children().pop_back();
138 this->iy_[IYI(ns->y())].pop_back();
143 newly_added.push_back(pn);
144 if (this->goal_found(pn, &KUWATA2008_DCOST)) {
145 this->tlog(this->findt());
151 if (this->samples().size() <= 1)
152 return this->goal_found();
153 for (auto na: newly_added) {
156 for (auto ns: this->steer(na, this->goal())) {
160 this->nodes().push_back(ns);
162 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
163 if (this->collide(pn, ns)) {
164 pn->children().pop_back();
166 this->iy_[IYI(ns->y())].pop_back();
171 if (this->goal_found(pn,
172 &KUWATA2008_DCOST)) {
173 this->tlog(this->findt());
180 return this->goal_found();
183 Karaman2011::Karaman2011(RRTNode *init, RRTNode *goal):
191 srand(static_cast<unsigned>(time(0)));
194 bool Karaman2011::next()
198 if (this->samples().size() == 0)
205 this->samples().push_back(rs);
207 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
209 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
212 std::vector<RRTNode *> nvs;
214 for (auto ns: this->steer(nn, rs)) {
225 this->nodes().size()),
234 this->nodes().size()),
237 this->nodes().push_back(ns);
240 if (!this->connect(pn, ns, nvs)) {
241 this->iy_[IYI(ns->y())].pop_back();
245 this->rewire(nvs, ns);
247 if (this->goal_found(pn, this->cost)) {
248 this->tlog(this->findt());
254 return this->goal_found();
257 bool Karaman2011::connect(
260 std::vector<RRTNode *> nvs)
262 RRTNode *op; // old parent
263 float od; // old direct cost
264 float oc; // old cumulative cost
265 bool connected = false;
266 pn->add_child(ns, this->cost(pn, ns));
267 if (this->collide(pn, ns)) {
268 pn->children().pop_back();
275 if (!connected || (nv->ccost() + this->cost(nv, ns) <
280 nv->add_child(ns, this->cost(nv, ns));
281 if (this->collide(nv, ns)) {
282 nv->children().pop_back();
289 } else if (connected) {
290 op->children().pop_back();
300 bool Karaman2011::rewire(std::vector<RRTNode *> nvs, RRTNode *ns)
302 RRTNode *op; // old parent
303 float od; // old direct cost
304 float oc; // old cumulative cost
306 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
310 ns->add_child(nv, this->cost(ns, nv));
311 if (this->collide(ns, nv)) {
312 ns->children().pop_back();
324 T1::T1(RRTNode *init, RRTNode *goal):
332 srand(static_cast<unsigned>(time(0)));
338 if (this->samples().size() == 0)
342 this->samples().push_back(rs);
344 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
346 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
349 std::vector<RRTNode *> nvs;
351 RRTNode *op; // old parent
352 float od; // old direct cost
353 float oc; // old cumulative cost
354 std::vector<RRTNode *> steered = this->steer(nn, rs);
355 // RRT* for first node
356 RRTNode *ns = steered[0];
359 nvs = this->nv(this->iy_, ns, this->cost, MIN(
360 GAMMA_RRTSTAR(this->nodes().size()),
363 nvs = this->nv(this->root(), ns, this->cost, MIN(
364 GAMMA_RRTSTAR(this->nodes().size()),
367 this->nodes().push_back(ns);
370 pn->add_child(ns, this->cost(pn, ns));
371 if (this->collide(pn, ns)) {
372 pn->children().pop_back();
378 if (!connected || (nv->ccost() + this->cost(nv, ns) <
383 nv->add_child(ns, this->cost(nv, ns));
384 if (this->collide(nv, ns)) {
385 nv->children().pop_back();
389 } else if (connected) {
390 op->children().pop_back();
400 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
404 ns->add_child(nv, this->cost(ns, nv));
405 if (this->collide(ns, nv)) {
406 ns->children().pop_back();
416 if (this->goal_found(pn, this->cost)) {
417 this->tlog(this->findt());
421 for (i = 1; i < steered.size(); i++) {
423 this->nodes().push_back(ns);
425 pn->add_child(ns, this->cost(pn, ns));
426 if (this->collide(pn, ns)) {
427 pn->children().pop_back();
431 if (this->goal_found(pn, this->cost)) {
432 this->tlog(this->findt());
436 return this->goal_found();
443 if (this->samples().size() == 0)
450 this->samples().push_back(rs);
452 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
454 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
457 std::vector<RRTNode *> nvs;
458 std::vector<RRTNode *> newly_added;
461 for (auto ns: this->steer(nn, rs)) {
464 } else if (IS_NEAR(pn, ns)) {
467 if (sgn(pn->s()) != sgn(ns->s()))
478 this->nodes().size()),
487 this->nodes().size()),
490 this->nodes().push_back(ns);
493 if (!this->connect(pn, ns, nvs)) {
494 this->iy_[IYI(ns->y())].pop_back();
498 this->rewire(nvs, ns);
500 newly_added.push_back(pn);
501 if (this->goal_found(pn, this->cost)) {
503 this->tlog(this->findt());
509 if (this->samples().size() <= 1)
510 return this->goal_found();
511 for (auto na: newly_added) {
515 for (auto ns: this->steer(na, this->goal())) {
518 } else if (IS_NEAR(pn, ns)) {
521 if (sgn(pn->s()) != sgn(ns->s()))
525 this->nodes().push_back(ns);
527 pn->add_child(ns, this->cost(pn, ns));
528 if (this->collide(pn, ns)) {
529 pn->children().pop_back();
531 this->iy_[IYI(ns->y())].pop_back();
536 if (this->goal_found(pn, this->cost)) {
538 this->tlog(this->findt());
545 return this->goal_found();
548 float T2::goal_cost()
550 std::vector<RRTNode *> nvs;
565 if (std::abs(this->goal()->h() - nv->h()) >=
566 this->GOAL_FOUND_ANGLE)
568 if (nv->ccost() + (*cost)(nv, this->goal()) >=
569 this->goal()->ccost())
571 RRTNode *op; // old parent
572 float oc; // old cumulative cost
573 float od; // old direct cost
574 op = this->goal()->parent();
575 oc = this->goal()->ccost();
576 od = this->goal()->dcost();
577 nv->add_child(this->goal(),
578 (*cost)(nv, this->goal()));
579 if (this->collide(nv, this->goal())) {
580 nv->children().pop_back();
581 this->goal()->parent(op);
582 this->goal()->ccost(oc);
583 this->goal()->dcost(od);
585 op->rem_child(this->goal());
588 return this->goal()->ccost();
591 class RRTNodeDijkstra {
593 RRTNodeDijkstra(int i):
599 RRTNodeDijkstra(int i, float c):
618 class RRTNodeDijkstraComparator {
621 const RRTNodeDijkstra& n1,
622 const RRTNodeDijkstra& n2)
630 if (this->tlog().size() == 0)
632 float oc = this->tlog().back().front()->ccost();
633 std::vector<RRTNode *> tmp_cusps;
634 for (auto n: this->tlog().back()) {
635 if (n->parent() && sgn(n->s()) != sgn(n->parent()->s())) {
636 tmp_cusps.push_back(n);
637 tmp_cusps.push_back(n->parent());
640 std::vector<RRTNode *> cusps;
641 for (unsigned int i = 0; i < tmp_cusps.size() - 1; i++) {
642 if (tmp_cusps[i] != tmp_cusps[i + 1])
643 cusps.push_back(tmp_cusps[i]);
645 cusps.push_back(this->root());
646 std::reverse(cusps.begin(), cusps.end());
647 cusps.push_back(this->tlog().back().front());
649 std::vector<RRTNodeDijkstra> dnodes;
650 for (unsigned int i = 0; i < cusps.size(); i++)
651 dnodes.push_back(RRTNodeDijkstra(i));
656 std::vector<RRTNodeDijkstra>,
657 RRTNodeDijkstraComparator> pq;
658 RRTNodeDijkstra tmp = dnodes[0];
660 float ch_cost = 9999;
661 std::vector<RRTNode *> steered;
662 while (!pq.empty() && tmp.ni != cusps.size() - 1) {
665 for (unsigned int i = tmp.ni + 1; i < cusps.size(); i++) {
666 ch_cost = dnodes[tmp.ni].c +
667 this->cost(cusps[tmp.ni], cusps[i]);
668 steered = this->steer(cusps[tmp.ni], cusps[i]);
669 for (unsigned int j = 0; j < steered.size() - 1; j++)
670 steered[j]->add_child(
677 steered[steered.size() - 1]))
679 if (ch_cost < dnodes[i].c) {
680 dnodes[i].c = ch_cost;
681 dnodes[i].pi = tmp.ni;
687 std::vector<int> npi; // new path indexes
691 tmpi = dnodes[tmpi].pi;
694 std::vector<RRTNode *> npn; // new path nodes
695 std::reverse(npi.begin(), npi.end());
696 RRTNode *pn = cusps[0];
697 for (unsigned int i = 0; i < npi.size() - 1; i++) {
698 for (auto ns: this->steer(cusps[npi[i]], cusps[npi[i + 1]])) {
699 pn->add_child(ns, this->cost(pn, ns));
704 this->tlog().back().front(),
705 this->cost(pn, this->tlog().back().front()));
707 if (this->tlog().back().front()->ccost() < oc)
712 bool T2::opt_part(RRTNode *init, RRTNode *goal)
714 std::vector<RRTNode *> steered;
715 steered = this->steer(init, goal);
716 unsigned int ss = steered.size();
717 for (unsigned int i = 0; i < ss - 1; i++) {
718 steered[i]->add_child(
722 steered[ss - 1])); // FIXME
724 if (this->collide(steered[0], steered[ss - 1])) {
725 for (auto n: steered)
733 op->add_child(steered[0], this->cost(op, steered[0]));
734 steered[ss - 1]->add_child(goal, this->cost(steered[ss - 1], goal));