base/rrtbase.cc

   1 /*
   2 This file is part of I am car.
   3
   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.
   8
   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.
  13
  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/>.
  16 */
  17
  18 #include <algorithm>
  19 #include <cmath>
  20 #include <omp.h>
  21 #include <queue>
  22 #include "bcar.h"
  23 #include "rrtbase.h"
  24 // OpenGL
  25 #include <GL/gl.h>
  26 #include <GL/glu.h>
  27 #include <SDL2/SDL.h>
  28 // RRT
  29 #include "cost.h"
  30 #include "steer.h"
  31
  32 extern SDL_Window* gw;
  33 extern SDL_GLContext gc;
  34
  35 RRTBase::~RRTBase()
  36 {
  37         for (auto n: this->nodes_)
  38                 if (n != this->root_)
  39                         delete n;
  40         for (auto n: this->dnodes_)
  41                 if (n != this->root_ && n != this->goal_)
  42                         delete n;
  43         for (auto s: this->samples_)
  44                 if (s != this->goal_)
  45                         delete s;
  46         for (auto edges: this->rlog_)
  47                 for (auto e: edges)
  48                         delete e;
  49         delete this->root_;
  50         delete this->goal_;
  51 }
  52
  53 RRTBase::RRTBase():
  54         root_(new RRTNode()),
  55         goal_(new RRTNode())
  56 {
  57         this->nodes_.push_back(this->root_);
  58         this->add_iy(this->root_);
  59 }
  60
  61 RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
  62         root_(init),
  63         goal_(goal)
  64 {
  65         this->nodes_.push_back(init);
  66         this->add_iy(init);
  67 }
  68
  69 // getter
  70 RRTNode *RRTBase::root()
  71 {
  72         return this->root_;
  73 }
  74
  75 RRTNode *RRTBase::goal()
  76 {
  77         return this->goal_;
  78 }
  79
  80 std::vector<RRTNode *> &RRTBase::nodes()
  81 {
  82         return this->nodes_;
  83 }
  84
  85 std::vector<RRTNode *> &RRTBase::dnodes()
  86 {
  87         return this->dnodes_;
  88 }
  89
  90 std::vector<RRTNode *> &RRTBase::samples()
  91 {
  92         return this->samples_;
  93 }
  94
  95 std::vector<CircleObstacle> *RRTBase::cos()
  96 {
  97         return this->cobstacles_;
  98 }
  99
 100 std::vector<SegmentObstacle> *RRTBase::sos()
 101 {
 102         return this->sobstacles_;
 103 }
 104
 105 std::vector<float> &RRTBase::clog()
 106 {
 107         return this->clog_;
 108 }
 109
 110 std::vector<float> &RRTBase::nlog()
 111 {
 112         return this->nlog_;
 113 }
 114
 115 std::vector<std::vector<RRTEdge *>> &RRTBase::rlog()
 116 {
 117         return this->rlog_;
 118 }
 119
 120 std::vector<float> &RRTBase::slog()
 121 {
 122         return this->slog_;
 123 }
 124
 125 std::vector<std::vector<RRTNode *>> &RRTBase::tlog()
 126 {
 127         return this->tlog_;
 128 }
 129
 130 bool RRTBase::goal_found()
 131 {
 132         return this->goal_found_;
 133 }
 134
 135 float RRTBase::elapsed()
 136 {
 137         std::chrono::duration<float> dt;
 138         dt = std::chrono::duration_cast<std::chrono::duration<float>>(
 139                         this->tend_ - this->tstart_);
 140         return dt.count();
 141 }
 142
 143 // setter
 144 void RRTBase::root(RRTNode *node)
 145 {
 146         this->root_ = node;
 147 }
 148
 149 void RRTBase::goal(RRTNode *node)
 150 {
 151         this->goal_ = node;
 152 }
 153
 154 bool RRTBase::logr(RRTNode *root)
 155 {
 156         std::vector<RRTEdge *> e; // Edges to log
 157         std::vector<RRTNode *> s; // DFS stack
 158         std::vector<RRTNode *> r; // reset visited_
 159         RRTNode *tmp;
 160         s.push_back(root);
 161         while (s.size() > 0) {
 162                 tmp = s.back();
 163                 s.pop_back();
 164                 if (!tmp->visit()) {
 165                         r.push_back(tmp);
 166                         for (auto ch: tmp->children()) {
 167                                 s.push_back(ch);
 168                                 e.push_back(new RRTEdge(tmp, ch));
 169                         }
 170                 }
 171         }
 172         for (auto n: r)
 173                 n->visit(false);
 174         this->rlog_.push_back(e);
 175         return true;
 176 }
 177
 178 float RRTBase::ocost(RRTNode *n)
 179 {
 180         float dist = 9999;
 181         for (auto o: *this->cobstacles_)
 182                 if (o.dist_to(n) < dist)
 183                         dist = o.dist_to(n);
 184         for (auto o: *this->sobstacles_)
 185                 if (o.dist_to(n) < dist)
 186                         dist = o.dist_to(n);
 187         return n->ocost(dist);
 188 }
 189
 190 bool RRTBase::tlog(std::vector<RRTNode *> t)
 191 {
 192         if (t.size() > 0) {
 193                 this->slog_.push_back(this->elapsed());
 194                 this->clog_.push_back(t.front()->ccost() - t.back()->ccost());
 195                 this->nlog_.push_back(this->nodes_.size());
 196                 this->tlog_.push_back(t);
 197                 return true;
 198         } else {
 199                 return false;
 200         }
 201 }
 202
 203 void RRTBase::tstart()
 204 {
 205         this->tstart_ = std::chrono::high_resolution_clock::now();
 206 }
 207
 208 void RRTBase::tend()
 209 {
 210         this->tend_ = std::chrono::high_resolution_clock::now();
 211 }
 212
 213 bool RRTBase::link_obstacles(
 214                 std::vector<CircleObstacle> *cobstacles,
 215                 std::vector<SegmentObstacle> *sobstacles)
 216 {
 217         this->cobstacles_ = cobstacles;
 218         this->sobstacles_ = sobstacles;
 219         if (!this->cobstacles_ || !this->sobstacles_) {
 220                 return false;
 221         }
 222         return true;
 223 }
 224
 225 bool RRTBase::add_iy(RRTNode *n)
 226 {
 227         int i = IYI(n->y());
 228         if (i < 0)
 229                 i = 0;
 230         if (i >= IYSIZE)
 231                 i = IYSIZE - 1;
 232         this->iy_[i].push_back(n);
 233         return true;
 234 }
 235
 236 bool RRTBase::goal_found(bool f)
 237 {
 238         this->goal_found_ = f;
 239         return f;
 240 }
 241
 242 // other
 243 bool RRTBase::glplot()
 244 {
 245         glClear(GL_COLOR_BUFFER_BIT);
 246         glLineWidth(1);
 247         glPointSize(1);
 248         // Plot obstacles
 249         glBegin(GL_LINES);
 250         for (auto o: *this->sobstacles_) {
 251                 glColor3f(0, 0, 0);
 252                 glVertex2f(GLVERTEX(o.init()));
 253                 glVertex2f(GLVERTEX(o.goal()));
 254         }
 255         glEnd();
 256         // Plot root, goal
 257         glPointSize(8);
 258         glBegin(GL_POINTS);
 259         glColor3f(1, 0, 0);
 260         glVertex2f(GLVERTEX(this->root_));
 261         glVertex2f(GLVERTEX(this->goal_));
 262         glEnd();
 263         // Plot last sample
 264         if (this->samples_.size() > 0) {
 265                 glPointSize(8);
 266                 glBegin(GL_POINTS);
 267                 glColor3f(0, 1, 0);
 268                 glVertex2f(GLVERTEX(this->samples_.back()));
 269                 glEnd();
 270         }
 271         // Plot nodes
 272         std::vector<RRTNode *> s; // DFS stack
 273         std::vector<RRTNode *> r; // reset visited_
 274         RRTNode *tmp;
 275         glBegin(GL_LINES);
 276         s.push_back(this->root_);
 277         while (s.size() > 0) {
 278                 tmp = s.back();
 279                 s.pop_back();
 280                 if (!tmp->visit()) {
 281                         r.push_back(tmp);
 282                         for (auto ch: tmp->children()) {
 283                                 s.push_back(ch);
 284                                 glColor3f(0.5, 0.5, 0.5);
 285                                 glVertex2f(GLVERTEX(tmp));
 286                                 glVertex2f(GLVERTEX(ch));
 287                         }
 288                 }
 289         }
 290         glEnd();
 291         std::vector<RRTNode *> cusps;
 292         // Plot last trajectory
 293         if (this->tlog().size() > 0) {
 294                 glLineWidth(2);
 295                 glBegin(GL_LINES);
 296                 for (auto n: this->tlog().back()) {
 297                         if (n->parent()) {
 298                                 glColor3f(0, 0, 1);
 299                                 glVertex2f(GLVERTEX(n));
 300                                 glVertex2f(GLVERTEX(n->parent()));
 301                                 if (sgn(n->s()) != sgn(n->parent()->s()))
 302                                         cusps.push_back(n);
 303                         }
 304                 }
 305                 glEnd();
 306         }
 307         // Plot cusps
 308         glPointSize(8);
 309         glBegin(GL_POINTS);
 310         for (auto n: cusps) {
 311                 glColor3f(0, 0, 1);
 312                 glVertex2f(GLVERTEX(n));
 313         }
 314         glEnd();
 315         SDL_GL_SwapWindow(gw);
 316         for (auto n: r)
 317                 n->visit(false);
 318         return true;
 319 }
 320
 321 bool RRTBase::goal_found(
 322                 RRTNode *node,
 323                 float (*cost)(RRTNode *, RRTNode* ))
 324 {
 325         float xx = pow(node->x() - this->goal_->x(), 2);
 326         float yy = pow(node->y() - this->goal_->y(), 2);
 327         float dh = std::abs(node->h() - this->goal_->h());
 328         if (IS_NEAR(node, this->goal_)) {
 329                 if (this->goal_found_) {
 330                         if (node->ccost() + (*cost)(node, this->goal_) <
 331                                         this->goal_->ccost()) {
 332                                 RRTNode *op; // old parent
 333                                 float oc; // old cumulative cost
 334                                 float od; // old direct cost
 335                                 op = this->goal_->parent();
 336                                 oc = this->goal_->ccost();
 337                                 od = this->goal_->dcost();
 338                                 node->add_child(this->goal_,
 339                                                 (*cost)(node, this->goal_));
 340                                 if (this->collide(node, this->goal_)) {
 341                                         node->children().pop_back();
 342                                         this->goal_->parent(op);
 343                                         this->goal_->ccost(oc);
 344                                         this->goal_->dcost(od);
 345                                 } else {
 346                                         op->rem_child(this->goal_);
 347                                         return true;
 348                                 }
 349                         } else {
 350                                 return false;
 351                         }
 352                 } else {
 353                         node->add_child(
 354                                         this->goal_,
 355                                         (*cost)(node, this->goal_));
 356                         if (this->collide(node, this->goal_)) {
 357                                 node->children().pop_back();
 358                                 this->goal_->remove_parent();
 359                                 return false;
 360                         }
 361                         this->goal_found_ = true;
 362                         return true;
 363                 }
 364         }
 365         return false;
 366 }
 367
 368 bool RRTBase::collide(RRTNode *init, RRTNode *goal)
 369 {
 370         std::vector<RRTEdge *> edges;
 371         RRTNode *tmp = goal;
 372         volatile bool col = false;
 373         unsigned int i;
 374         while (tmp != init) {
 375                 BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
 376                 std::vector<RRTEdge *> bcframe = bc.frame();
 377                 #pragma omp parallel for reduction(|: col)
 378                 for (i = 0; i < (*this->cobstacles_).size(); i++) {
 379                         if ((*this->cobstacles_)[i].collide(tmp)) {
 380                                 col = true;
 381                         }
 382                         for (auto &e: bcframe) {
 383                                 if ((*this->cobstacles_)[i].collide(e)) {
 384                                         col = true;
 385                                 }
 386                         }
 387                 }
 388                 if (col) {
 389                         for (auto e: bcframe) {
 390                                 delete e->init();
 391                                 delete e->goal();
 392                                 delete e;
 393                         }
 394                         for (auto e: edges) {
 395                                 delete e;
 396                         }
 397                         return true;
 398                 }
 399                 #pragma omp parallel for reduction(|: col)
 400                 for (i = 0; i < (*this->sobstacles_).size(); i++) {
 401                         for (auto &e: bcframe) {
 402                                 if ((*this->sobstacles_)[i].collide(e)) {
 403                                         col = true;
 404                                 }
 405                         }
 406                 }
 407                 if (col) {
 408                         for (auto e: bcframe) {
 409                                 delete e->init();
 410                                 delete e->goal();
 411                                 delete e;
 412                         }
 413                         for (auto e: edges) {
 414                                 delete e;
 415                         }
 416                         return true;
 417                 }
 418                 if (!tmp->parent()) {
 419                         break;
 420                 }
 421                 edges.push_back(new RRTEdge(tmp, tmp->parent()));
 422                 tmp = tmp->parent();
 423                 for (auto e: bcframe) {
 424                         delete e->init();
 425                         delete e->goal();
 426                         delete e;
 427                 }
 428         }
 429         for (auto &e: edges) {
 430                 #pragma omp parallel for reduction(|: col)
 431                 for (i = 0; i < (*this->cobstacles_).size(); i++) {
 432                         if ((*this->cobstacles_)[i].collide(e)) {
 433                                 col = true;
 434                         }
 435                 }
 436                 if (col) {
 437                         for (auto e: edges) {
 438                                 delete e;
 439                         }
 440                         return true;
 441                 }
 442                 #pragma omp parallel for reduction(|: col)
 443                 for (i = 0; i < (*this->sobstacles_).size(); i++) {
 444                         if ((*this->sobstacles_)[i].collide(e)) {
 445                                 col = true;
 446                         }
 447                 }
 448                 if (col) {
 449                         for (auto e: edges) {
 450                                 delete e;
 451                         }
 452                         return true;
 453                 }
 454         }
 455         for (auto e: edges) {
 456                 delete e;
 457         }
 458         return false;
 459 }
 460
 461 class RRTNodeDijkstra {
 462         public:
 463                 RRTNodeDijkstra(int i):
 464                         ni(i),
 465                         pi(0),
 466                         c(9999),
 467                         v(false)
 468                 {};
 469                 RRTNodeDijkstra(int i, float c):
 470                         ni(i),
 471                         pi(0),
 472                         c(c),
 473                         v(false)
 474                 {};
 475                 RRTNodeDijkstra(int i, int p, float c):
 476                         ni(i),
 477                         pi(p),
 478                         c(c),
 479                         v(false)
 480                 {};
 481                 unsigned int ni;
 482                 unsigned int pi;
 483                 float c;
 484                 bool v;
 485                 bool vi()
 486                 {
 487                         if (this->v)
 488                                 return true;
 489                         this->v = true;
 490                         return false;
 491                 };
 492 };
 493
 494 class RRTNodeDijkstraComparator {
 495         public:
 496                 int operator() (
 497                                 const RRTNodeDijkstra& n1,
 498                                 const RRTNodeDijkstra& n2)
 499                 {
 500                         return n1.c > n2.c;
 501                 }
 502 };
 503
 504 bool RRTBase::opt_path()
 505 {
 506         if (this->tlog().size() == 0)
 507                 return false;
 508         float oc = this->tlog().back().front()->ccost();
 509         std::vector<RRTNode *> tmp_cusps;
 510         for (auto n: this->tlog().back()) {
 511                 if (sgn(n->s()) == 0) {
 512                         tmp_cusps.push_back(n);
 513                 } else if (n->parent() &&
 514                                 sgn(n->s()) != sgn(n->parent()->s())) {
 515                         tmp_cusps.push_back(n);
 516                         tmp_cusps.push_back(n->parent());
 517                 }
 518         }
 519         if (tmp_cusps.size() < 2)
 520                 return false;
 521         std::vector<RRTNode *> cusps;
 522         for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
 523                 if (tmp_cusps[i] != tmp_cusps[i + 1])
 524                         cusps.push_back(tmp_cusps[i]);
 525         }
 526         std::reverse(cusps.begin(), cusps.end());
 527         // Begin of Dijkstra
 528         std::vector<RRTNodeDijkstra> dnodes;
 529         for (unsigned int i = 0; i < cusps.size(); i++)
 530                 if (i > 0)
 531                         dnodes.push_back(RRTNodeDijkstra(
 532                                                 i,
 533                                                 i - 1,
 534                                                 cusps[i]->ccost()));
 535                 else
 536                         dnodes.push_back(RRTNodeDijkstra(
 537                                                 i,
 538                                                 cusps[i]->ccost()));
 539         dnodes[0].vi();
 540         std::priority_queue<
 541                 RRTNodeDijkstra,
 542                 std::vector<RRTNodeDijkstra>,
 543                 RRTNodeDijkstraComparator> pq;
 544         RRTNodeDijkstra tmp = dnodes[0];
 545         pq.push(tmp);
 546         float ch_cost = 9999;
 547         std::vector<RRTNode *> steered;
 548         while (!pq.empty() && tmp.ni != cusps.size() - 1) {
 549                 tmp = pq.top();
 550                 pq.pop();
 551                 for (unsigned int i = tmp.ni + 1; i < cusps.size(); i++) {
 552                         ch_cost = dnodes[tmp.ni].c +
 553                                 CO(cusps[tmp.ni], cusps[i]);
 554                         steered = ST(cusps[tmp.ni], cusps[i]);
 555                         for (unsigned int j = 0; j < steered.size() - 1; j++)
 556                                 steered[j]->add_child(
 557                                                 steered[j + 1],
 558                                                 CO(
 559                                                         steered[j],
 560                                                         steered[j + 1]));
 561                         if (i != tmp.ni + 1 && this->collide( // TODO
 562                                                 steered[0],
 563                                                 steered[steered.size() - 1]))
 564                                 continue;
 565                         if (ch_cost < dnodes[i].c) {
 566                                 dnodes[i].c = ch_cost;
 567                                 dnodes[i].pi = tmp.ni;
 568                                 if (!dnodes[i].vi())
 569                                         pq.push(dnodes[i]);
 570                         }
 571                 }
 572         }
 573         if (tmp.ni != cusps.size() - 1)
 574                 return false;
 575         std::vector<int> npi; // new path indexes
 576         int tmpi = tmp.ni;
 577         while (tmpi > 0) {
 578                 npi.push_back(tmpi);
 579                 tmpi = dnodes[tmpi].pi;
 580         }
 581         npi.push_back(tmpi);
 582         std::reverse(npi.begin(), npi.end());
 583         RRTNode *pn = cusps[npi[0]];
 584         for (unsigned int i = 0; i < npi.size() - 1; i++) {
 585                 for (auto ns: ST(cusps[npi[i]], cusps[npi[i + 1]])) {
 586                         pn->add_child(ns, CO(pn, ns));
 587                         pn = ns;
 588                 }
 589         }
 590         pn->add_child(
 591                         this->tlog().back().front(),
 592                         CO(pn, this->tlog().back().front()));
 593         // End of Dijkstra
 594         if (this->tlog().back().front()->ccost() < oc)
 595                 return true;
 596         return false;
 597 }
 598
 599 bool RRTBase::rebase(RRTNode *nr)
 600 {
 601         if (!nr || this->goal_ == nr || this->root_ == nr)
 602                 return false;
 603         std::vector<RRTNode *> s; // DFS stack
 604         RRTNode *tmp;
 605         unsigned int i = 0;
 606         unsigned int to_del = 0;
 607         int iy = 0;
 608         s.push_back(this->root_);
 609         while (s.size() > 0) {
 610                 tmp = s.back();
 611                 s.pop_back();
 612                 for (auto ch: tmp->children()) {
 613                         if (ch != nr)
 614                                 s.push_back(ch);
 615                 }
 616                 to_del = this->nodes_.size();
 617                 #pragma omp parallel for reduction(min: to_del)
 618                 for (i = 0; i < this->nodes_.size(); i++) {
 619                         if (this->nodes_[i] == tmp)
 620                                 to_del = i;
 621                 }
 622                 if (to_del < this->nodes_.size())
 623                         this->nodes_.erase(this->nodes_.begin() + to_del);
 624 #if NNVERSION > 1
 625                 iy = IYI(tmp->y());
 626                 to_del = this->iy_[iy].size();
 627                 #pragma omp parallel  for reduction(min: to_del)
 628                 for (i = 0; i < this->iy_[iy].size(); i++) {
 629                         if (this->iy_[iy][i] == tmp)
 630                                 to_del = i;
 631                 }
 632                 if (to_del < this->iy_[iy].size())
 633                         this->iy_[iy].erase(this->iy_[iy].begin() + to_del);
 634 #endif
 635                 this->dnodes().push_back(tmp);
 636         }
 637         this->root_ = nr;
 638         this->root_->remove_parent();
 639         return true;
 640 }
 641
 642 std::vector<RRTNode *> RRTBase::findt()
 643 {
 644         return this->findt(this->goal_);
 645 }
 646
 647 std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 648 {
 649         std::vector<RRTNode *> nodes;
 650         if (!n || !n->parent())
 651                 return nodes;
 652         RRTNode *tmp = n;
 653         while (tmp != this->root()) {
 654                 nodes.push_back(tmp);
 655                 tmp = tmp->parent();
 656         }
 657         return nodes;
 658 }