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/>.
25 #include "rrtplanner.h"
28 #define CATI(a, b) a ## b
29 #define CAT(a, b) CATI(a, b)
30 #define KUWATA2008_CCOST CAT(c, CO)
31 #define KUWATA2008_DCOST CO
33 LaValle1998::LaValle1998(RRTNode *init, RRTNode *goal):
40 srand(static_cast<unsigned>(time(0)));
43 bool LaValle1998::next()
47 if (this->samples().size() == 0)
54 this->samples().push_back(rs);
56 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
58 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
62 for (auto ns: this->steer(nn, rs)) {
66 this->nodes().push_back(ns);
68 pn->add_child(ns, this->cost(pn, ns));
69 if (this->collide(pn, ns)) {
70 pn->children().pop_back();
72 this->iy_[IYI(ns->y())].pop_back();
77 if (this->goal_found(pn, this->cost)) {
78 this->tlog(this->findt());
84 return this->goal_found();
87 Kuwata2008::Kuwata2008(RRTNode *init, RRTNode *goal):
92 cost(KUWATA2008_DCOST)
94 srand(static_cast<unsigned>(time(0)));
97 bool Kuwata2008::next()
100 if (this->samples().size() == 0) {
105 this->samples().push_back(rs);
106 float heur = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
107 if (this->goal_found()) {
109 this->cost = &KUWATA2008_CCOST;
111 this->cost = &KUWATA2008_DCOST;
114 this->cost = &KUWATA2008_CCOST;
116 this->cost = &KUWATA2008_DCOST;
119 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
121 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
124 std::vector<RRTNode *> newly_added;
126 for (auto ns: this->steer(nn, rs)) {
130 this->nodes().push_back(ns);
132 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
133 if (this->collide(pn, ns)) {
134 pn->children().pop_back();
136 this->iy_[IYI(ns->y())].pop_back();
141 newly_added.push_back(pn);
142 if (this->goal_found(pn, &KUWATA2008_DCOST)) {
143 this->tlog(this->findt());
149 if (this->samples().size() <= 1)
150 return this->goal_found();
151 for (auto na: newly_added) {
154 for (auto ns: this->steer(na, this->goal())) {
158 this->nodes().push_back(ns);
160 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
161 if (this->collide(pn, ns)) {
162 pn->children().pop_back();
164 this->iy_[IYI(ns->y())].pop_back();
169 if (this->goal_found(pn,
170 &KUWATA2008_DCOST)) {
171 this->tlog(this->findt());
178 return this->goal_found();
181 Karaman2011::Karaman2011(RRTNode *init, RRTNode *goal):
189 srand(static_cast<unsigned>(time(0)));
192 bool Karaman2011::next()
196 if (this->samples().size() == 0)
203 this->samples().push_back(rs);
205 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
207 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
210 std::vector<RRTNode *> nvs;
212 for (auto ns: this->steer(nn, rs)) {
223 this->nodes().size()),
232 this->nodes().size()),
235 this->nodes().push_back(ns);
238 if (!this->connect(pn, ns, nvs)) {
239 this->iy_[IYI(ns->y())].pop_back();
243 this->rewire(nvs, ns);
245 if (this->goal_found(pn, this->cost)) {
246 this->tlog(this->findt());
252 return this->goal_found();
255 bool Karaman2011::connect(
258 std::vector<RRTNode *> nvs)
260 RRTNode *op; // old parent
261 float od; // old direct cost
262 float oc; // old cumulative cost
263 bool connected = false;
264 pn->add_child(ns, this->cost(pn, ns));
265 if (this->collide(pn, ns)) {
266 pn->children().pop_back();
273 if (!connected || (nv->ccost() + this->cost(nv, ns) <
278 nv->add_child(ns, this->cost(nv, ns));
279 if (this->collide(nv, ns)) {
280 nv->children().pop_back();
287 } else if (connected) {
288 op->children().pop_back();
298 bool Karaman2011::rewire(std::vector<RRTNode *> nvs, RRTNode *ns)
300 RRTNode *op; // old parent
301 float od; // old direct cost
302 float oc; // old cumulative cost
304 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
308 ns->add_child(nv, this->cost(ns, nv));
309 if (this->collide(ns, nv)) {
310 ns->children().pop_back();
322 T1::T1(RRTNode *init, RRTNode *goal):
330 srand(static_cast<unsigned>(time(0)));
336 if (this->samples().size() == 0)
340 this->samples().push_back(rs);
342 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
344 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
347 std::vector<RRTNode *> nvs;
349 RRTNode *op; // old parent
350 float od; // old direct cost
351 float oc; // old cumulative cost
352 std::vector<RRTNode *> steered = this->steer(nn, rs);
353 // RRT* for first node
354 RRTNode *ns = steered[0];
357 nvs = this->nv(this->iy_, ns, this->cost, MIN(
358 GAMMA_RRTSTAR(this->nodes().size()),
361 nvs = this->nv(this->root(), ns, this->cost, MIN(
362 GAMMA_RRTSTAR(this->nodes().size()),
365 this->nodes().push_back(ns);
368 pn->add_child(ns, this->cost(pn, ns));
369 if (this->collide(pn, ns)) {
370 pn->children().pop_back();
376 if (!connected || (nv->ccost() + this->cost(nv, ns) <
381 nv->add_child(ns, this->cost(nv, ns));
382 if (this->collide(nv, ns)) {
383 nv->children().pop_back();
387 } else if (connected) {
388 op->children().pop_back();
398 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
402 ns->add_child(nv, this->cost(ns, nv));
403 if (this->collide(ns, nv)) {
404 ns->children().pop_back();
414 if (this->goal_found(pn, this->cost)) {
415 this->tlog(this->findt());
419 for (i = 1; i < steered.size(); i++) {
421 this->nodes().push_back(ns);
423 pn->add_child(ns, this->cost(pn, ns));
424 if (this->collide(pn, ns)) {
425 pn->children().pop_back();
429 if (this->goal_found(pn, this->cost)) {
430 this->tlog(this->findt());
434 return this->goal_found();
441 if (this->samples().size() == 0)
448 this->samples().push_back(rs);
450 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
452 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
455 std::vector<RRTNode *> nvs;
456 std::vector<RRTNode *> newly_added;
459 for (auto ns: this->steer(nn, rs)) {
462 } else if (IS_NEAR(pn, ns)) {
464 } else if (cusps > 0) {
467 if (sgn(pn->s()) != sgn(ns->s()))
476 this->nodes().size()),
485 this->nodes().size()),
488 this->nodes().push_back(ns);
491 if (!this->connect(pn, ns, nvs)) {
492 this->iy_[IYI(ns->y())].pop_back();
496 this->rewire(nvs, ns);
498 newly_added.push_back(pn);
499 if (this->goal_found(pn, this->cost)) {
501 this->tlog(this->findt());
507 if (this->samples().size() <= 1)
508 return this->goal_found();
509 for (auto na: newly_added) {
513 for (auto ns: this->steer(na, this->goal())) {
516 } else if (IS_NEAR(pn, ns)) {
518 } else if (cusps > 0) {
521 if (sgn(pn->s()) != sgn(ns->s()))
523 this->nodes().push_back(ns);
525 pn->add_child(ns, this->cost(pn, ns));
526 if (this->collide(pn, ns)) {
527 pn->children().pop_back();
529 this->iy_[IYI(ns->y())].pop_back();
534 if (this->goal_found(pn, this->cost)) {
536 this->tlog(this->findt());
543 return this->goal_found();
546 float T2::goal_cost()
548 std::vector<RRTNode *> nvs;
563 if (std::abs(this->goal()->h() - nv->h()) >=
564 this->GOAL_FOUND_ANGLE)
566 if (nv->ccost() + (*cost)(nv, this->goal()) >=
567 this->goal()->ccost())
569 RRTNode *op; // old parent
570 float oc; // old cumulative cost
571 float od; // old direct cost
572 op = this->goal()->parent();
573 oc = this->goal()->ccost();
574 od = this->goal()->dcost();
575 nv->add_child(this->goal(),
576 (*cost)(nv, this->goal()));
577 if (this->collide(nv, this->goal())) {
578 nv->children().pop_back();
579 this->goal()->parent(op);
580 this->goal()->ccost(oc);
581 this->goal()->dcost(od);
583 op->rem_child(this->goal());
586 return this->goal()->ccost();