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) <
276 if (op && std::abs(op->t() - nv->t()) > 0.2)
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()) {
308 if (op && std::abs(op->t() - ns->t()) > 0.2)
312 ns->add_child(nv, this->cost(ns, nv));
313 if (this->collide(ns, nv)) {
314 ns->children().pop_back();
326 T1::T1(RRTNode *init, RRTNode *goal):
334 srand(static_cast<unsigned>(time(0)));
340 if (this->samples().size() == 0)
344 this->samples().push_back(rs);
346 RRTNode *nn = this->nn(this->iy_, rs, this->cost);
348 RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
351 std::vector<RRTNode *> nvs;
353 RRTNode *op; // old parent
354 float od; // old direct cost
355 float oc; // old cumulative cost
356 std::vector<RRTNode *> steered = this->steer(nn, rs);
357 // RRT* for first node
358 RRTNode *ns = steered[0];
361 nvs = this->nv(this->iy_, ns, this->cost, MIN(
362 GAMMA_RRTSTAR(this->nodes().size()),
365 nvs = this->nv(this->root(), ns, this->cost, MIN(
366 GAMMA_RRTSTAR(this->nodes().size()),
369 this->nodes().push_back(ns);
372 pn->add_child(ns, this->cost(pn, ns));
373 if (this->collide(pn, ns)) {
374 pn->children().pop_back();
380 if (!connected || (nv->ccost() + this->cost(nv, ns) <
385 nv->add_child(ns, this->cost(nv, ns));
386 if (this->collide(nv, ns)) {
387 nv->children().pop_back();
391 } else if (connected) {
392 op->children().pop_back();
402 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
406 ns->add_child(nv, this->cost(ns, nv));
407 if (this->collide(ns, nv)) {
408 ns->children().pop_back();
418 if (this->goal_found(pn, this->cost)) {
419 this->tlog(this->findt());
423 for (i = 1; i < steered.size(); i++) {
425 this->nodes().push_back(ns);
427 pn->add_child(ns, this->cost(pn, ns));
428 if (this->collide(pn, ns)) {
429 pn->children().pop_back();
433 if (this->goal_found(pn, this->cost)) {
434 this->tlog(this->findt());
438 return this->goal_found();