decision_control/slotplanner.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 <list>
  21 #include <queue>
  22 #include "slotplanner.h"
  23
  24 ParallelSlot::ParallelSlot()
  25 {}
  26
  27 // getter
  28 std::vector<std::vector<RRTNode *>> &ParallelSlot::cusp()
  29 {
  30         return this->cusp_;
  31 }
  32
  33 float ParallelSlot::DH() const
  34 {
  35         return this->DH_;
  36 }
  37
  38 PolygonObstacle &ParallelSlot::slot()
  39 {
  40         return this->slot_;
  41 }
  42
  43 float ParallelSlot::slotHeading()
  44 {
  45         float y0 = this->slot().bnodes()[0]->y();
  46         float x0 = this->slot().bnodes()[0]->x();
  47         float y3 = this->slot().bnodes()[3]->y();
  48         float x3 = this->slot().bnodes()[3]->x();
  49         float dy = y0 - y3;
  50         float dx = x0 - x3;
  51         return atan2(dy, dx);
  52 }
  53
  54 SlotSide ParallelSlot::slotSide()
  55 {
  56         return this->slotSide_;
  57 }
  58
  59 SlotType ParallelSlot::slotType()
  60 {
  61         return this->slotType_;
  62 }
  63
  64 // setter
  65 void ParallelSlot::DH(float dh)
  66 {
  67         this->DH_ = dh;
  68 }
  69
  70 void ParallelSlot::setAll()
  71 {
  72         // slot side
  73         float y0 = this->slot().bnodes()[0]->y();
  74         float x0 = this->slot().bnodes()[0]->x();
  75         float y1 = this->slot().bnodes()[1]->y();
  76         float x1 = this->slot().bnodes()[1]->x();
  77         float y3 = this->slot().bnodes()[3]->y();
  78         float x3 = this->slot().bnodes()[3]->x();
  79         if (sgn((x1 - x3) * (y0 - y3) - (y1 - y3) * (x0 - x3)) < 0)
  80                 this->slotSide_ = LEFT;
  81         else
  82                 this->slotSide_ = RIGHT;
  83         // slot type
  84         float d1 = EDIST(
  85                 this->slot().bnodes()[0],
  86                 this->slot().bnodes()[1]
  87         );
  88         float d2 = EDIST(
  89                 this->slot().bnodes()[1],
  90                 this->slot().bnodes()[2]
  91         );
  92         if (d1 > d2)
  93                 this->slotType_ = PERPENDICULAR;
  94         else
  95                 this->slotType_ = PARALLEL;
  96 }
  97
  98 // other
  99 void ParallelSlot::fip(
 100         std::vector<CircleObstacle>& co,
 101         std::vector<SegmentObstacle>& so
 102 )
 103 {
 104         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
 105         // RRTNode.s() works as iteration level
 106         std::queue<BicycleCar *, std::list<BicycleCar *>> q;
 107         std::queue<BicycleCar *, std::list<BicycleCar *>> empty;
 108         int di = -1;
 109         if (this->slotSide() == LEFT)
 110                 di = 1;
 111         BicycleCar *CC = this->getEPC();
 112         BicycleCar *B = this->getEP();
 113         this->DH(di * 0.01 / CC->out_radi());
 114         BicycleCar *c;
 115         int i = 0;
 116         c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 117         while (!this->slot().collide(c->frame())) {
 118                 bool end = false;
 119                 std::vector<RRTEdge *> eds = c->frame();
 120                 for (auto o: co)
 121                         if (o.collide(eds))
 122                                 end = true;
 123                 for (auto o: so)
 124                         if (o.collide(eds))
 125                                 end = true;
 126                 for (auto e: eds)
 127                         delete e;
 128                 if (!end)
 129                         q.push(c);
 130                 c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 131                 i += 1;
 132         }
 133         delete c; // not in q and collide
 134         while (!q.empty()) {
 135                 c = q.front();
 136                 q.pop();
 137                 if (this->isInside(c)) {
 138                         goto createcuspandfinish;
 139                 } else if (c->s() < 9) {
 140                         BicycleCar *cc = this->flnc(c, co, so);
 141                         cc->s(c->s() + 1);
 142                         cc->bcparent(c);
 143                         q.push(cc);
 144                 } else {
 145                         delete c; // not in q and collide
 146                 }
 147         }
 148         std::swap(q, empty);
 149         return;
 150 createcuspandfinish:
 151         std::vector<RRTNode *> cusp;
 152         while (c) {
 153                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 154                 c = c->bcparent();
 155         }
 156         std::reverse(cusp.begin(), cusp.end());
 157         this->cusp().push_back(cusp);
 158         std::swap(q, empty);
 159 }
 160
 161 void ParallelSlot::fipr(RRTNode *n)
 162 {
 163         return this->fipr(new BicycleCar(n->x(), n->y(), n->h()));
 164 }
 165
 166 void ParallelSlot::fipr(BicycleCar *B)
 167 {
 168         std::vector<RRTNode *> cusp;
 169         cusp.push_back(new RRTNode(B->x(), B->y(), B->h()));
 170         int di = 1;
 171         if (this->slotSide() == LEFT)
 172                 di = -1;
 173         if (this->slotType() == PERPENDICULAR) {
 174                 this->DH(di * 0.01 / B->out_radi()); // TODO car in slot h()
 175                 RRTNode *cc;
 176                 if (this->slotSide() == LEFT)
 177                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 178                 else
 179                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 180                 BicycleCar *p;
 181                 int i = 1;
 182                 p = B->move(cc, i * this->DH());
 183                 while (
 184                         !this->slot().collide(p->frame())
 185                         && this->slot().collide(p)
 186                 ) {
 187                         delete p;
 188                         i += 10;
 189                         p = B->move(cc, i * this->DH());
 190                 }
 191                 i -= 10;
 192                 p = B->move(cc, i * this->DH());
 193                 while (
 194                         !this->slot().collide(p->frame())
 195                         && this->slot().collide(p)
 196                 ) {
 197                         delete p;
 198                         i += 1;
 199                         p = B->move(cc, i * this->DH());
 200                 }
 201                 i -= 1;
 202                 p = B->move(cc, i * this->DH());
 203                 cusp.push_back(new RRTNode(p->x(), p->y(), p->h()));
 204                 std::reverse(cusp.begin(), cusp.end());
 205                 this->cusp().push_back(cusp);
 206                 return;
 207         }
 208         this->DH(di * 0.01 / B->out_radi());
 209         BicycleCar *c;
 210         c = this->flncr(B);
 211         c->s(B->s() + 1);
 212         while ((
 213                 this->slotSide() == LEFT
 214                 && this->slot().collide(new RRTNode(c->lfx(), c->lfy(), 0))
 215         ) || (
 216                 this->slotSide() == RIGHT
 217                 && this->slot().collide(new RRTNode(c->rfx(), c->rfy(), 0))
 218         )) {
 219                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 220                 BicycleCar *cc = this->flncr(c);
 221                 cc->s(c->s() + 1);
 222                 delete c;
 223                 c = cc;
 224         }
 225         cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 226         std::reverse(cusp.begin(), cusp.end());
 227         this->cusp().push_back(cusp);
 228 }
 229
 230 BicycleCar *ParallelSlot::flnc(
 231         BicycleCar *B,
 232         std::vector<CircleObstacle>& co,
 233         std::vector<SegmentObstacle>& so
 234 )
 235 {
 236         RRTNode *cc;
 237         if (this->slotSide() == LEFT) {
 238                 if (int(B->s()) % 2 == 0)
 239                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 240                 else
 241                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 242         } else {
 243                 if (int(B->s()) % 2 == 0)
 244                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 245                 else
 246                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 247         }
 248         BicycleCar *p;
 249         int i = 1;
 250         p = B->move(cc, i * this->DH());
 251         while (
 252                 !this->slot().collide(p->frame())
 253                 && std::abs(this->slotHeading() - p->h()) < M_PI / 2
 254         ) {
 255                 delete p;
 256                 i += 10;
 257                 p = B->move(cc, i * this->DH());
 258                 bool end = false;
 259                 std::vector<RRTEdge *> eds = p->frame();
 260                 for (auto o: co)
 261                         if (o.collide(eds))
 262                                 end = true;
 263                 for (auto o: so)
 264                         if (o.collide(eds))
 265                                 end = true;
 266                 for (auto e: eds)
 267                         delete e;
 268                 if (end)
 269                         break;
 270         }
 271         i -= 10;
 272         p = B->move(cc, i * this->DH());
 273         while (
 274                 !this->slot().collide(p->frame())
 275                 && std::abs(this->slotHeading() - p->h()) < M_PI / 2
 276         ) {
 277                 if (this->isInside(p)) {
 278                         i += 1;
 279                         break;
 280                 }
 281                 delete p;
 282                 i += 1;
 283                 p = B->move(cc, i * this->DH());
 284                 bool end = false;
 285                 std::vector<RRTEdge *> eds = p->frame();
 286                 for (auto o: co)
 287                         if (o.collide(eds))
 288                                 end = true;
 289                 for (auto o: so)
 290                         if (o.collide(eds))
 291                                 end = true;
 292                 for (auto e: eds)
 293                         delete e;
 294                 if (end)
 295                         break;
 296         }
 297         delete p;
 298         return B->move(cc, (i - 1) * this->DH());
 299 }
 300
 301 BicycleCar *ParallelSlot::flncr(BicycleCar *B)
 302 {
 303         RRTNode *cc;
 304         if (this->slotSide() == LEFT) {
 305                 if (int(B->s()) % 2 == 0)
 306                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 307                 else
 308                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 309         } else {
 310                 if (int(B->s()) % 2 == 0)
 311                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 312                 else
 313                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 314         }
 315         BicycleCar *p;
 316         int i = 1;
 317         p = B->move(cc, i * this->DH());
 318         while (
 319                 !this->slot().collide(p->frame())
 320                 && ((
 321                         this->slotSide() == LEFT
 322                         && this->slot().collide(new RRTNode(
 323                                 p->lfx(),
 324                                 p->lfy(),
 325                                 0
 326                         ))
 327                 ) || (
 328                         this->slotSide() == RIGHT
 329                         && this->slot().collide(new RRTNode(
 330                                 p->rfx(),
 331                                 p->rfy(),
 332                                 0
 333                         ))
 334                 ))
 335         ) {
 336                 delete p;
 337                 i += 10;
 338                 p = B->move(cc, i * this->DH());
 339         }
 340         i -= 10;
 341         p = B->move(cc, i * this->DH());
 342         while (!this->slot().collide(p->frame())) {
 343                 if(
 344                         this->slotSide() == LEFT
 345                         && !this->slot().collide(new RRTNode(
 346                                 p->lfx(),
 347                                 p->lfy(),
 348                                 0
 349                         ))
 350                 ) {
 351                         i += 1;
 352                         break;
 353                 }
 354                 if(
 355                         this->slotSide() == RIGHT
 356                         && !this->slot().collide(new RRTNode(
 357                                 p->rfx(),
 358                                 p->rfy(),
 359                                 0
 360                         ))
 361                 ) {
 362                         i += 1;
 363                         break;
 364                 }
 365                 i += 1;
 366                 p = B->move(cc, i * this->DH());
 367         }
 368         delete p;
 369         return B->move(cc, (i - 1) * this->DH());
 370 }
 371
 372 RRTNode *ParallelSlot::fposecenter()
 373 {
 374         return this->slot().bnodes().front();
 375 }
 376
 377 bool ParallelSlot::flast(
 378         RRTNode *P,
 379         bool right,
 380         int il,
 381         std::vector<RRTNode *> &cusp
 382 )
 383 {
 384         BicycleCar *B = new BicycleCar(P->x(), P->y(), P->h());
 385         RRTNode *cc;
 386         if (right)
 387                 cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 388         else
 389                 cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 390         BicycleCar *p;
 391         int i = 1;
 392         p = B->move(cc, i * this->DH());
 393         while (!this->slot().collide(p->frame())
 394                         && (
 395                                 (this->DH() > 0 && p->x() <= 0)
 396                                 || (this->DH() < 0 && p->x() >= 0)
 397                         )) {
 398                 delete p;
 399                 i += 10;
 400                 p = B->move(cc, i * this->DH());
 401         }
 402         i -= 10;
 403         p = B->move(cc, i * this->DH());
 404         while (!this->slot().collide(p->frame())
 405                         && (
 406                                 (this->DH() > 0 && p->x() <= 0)
 407                                 || (this->DH() < 0 && p->x() >= 0)
 408                         )) {
 409                 if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 410                         i += 1;
 411                         break;
 412                 }
 413                 if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 414                         i += 1;
 415                         break;
 416                 }
 417                 delete p;
 418                 i += 1;
 419                 p = B->move(cc, i * this->DH());
 420         }
 421         delete p;
 422         p = B->move(cc, (i - 1) * this->DH());
 423         if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 424                 cusp.push_back(p);
 425                 return true;
 426         } else if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 427                 cusp.push_back(p);
 428                 return true;
 429         } else if (il < 8) {
 430                 cusp.push_back(p);
 431                 return this->flast(p, !right, il + 1, cusp);
 432         }
 433         return false;
 434 }
 435
 436 void ParallelSlot::fpose()
 437 {
 438         bool left = false; // right parking slot
 439         float di = -1;
 440         BicycleCar *CC = new BicycleCar(
 441                 this->fposecenter()->x(),
 442                 this->fposecenter()->y() - 0.01,
 443                 this->slotHeading()
 444         );
 445         BicycleCar *B = new BicycleCar(
 446                 CC->x() - CC->width() / 2,
 447                 CC->y() - (CC->length() + CC->wheelbase()) / 2,
 448                 this->slotHeading()
 449         );
 450         if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
 451                 left = true;
 452                 di = 1;
 453                 delete B;
 454                 B = new BicycleCar(
 455                         CC->x() + CC->width() / 2,
 456                         CC->y() - (CC->length() + CC->wheelbase()) / 2,
 457                         this->slotHeading()
 458                 );
 459         }
 460         this->DH(di * 0.01 / CC->out_radi());
 461         BicycleCar *p;
 462         int i = 0;
 463         p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 464         while (!this->slot().collide(p->frame())) {
 465                 std::vector<RRTNode *> tmpcusp;
 466                 tmpcusp.push_back(new BicycleCar(p->x(), p->y(), p->h()));
 467                 if (this->flast(p, left, 0, tmpcusp)) {
 468                         this->cusp().push_back(tmpcusp);
 469                         return;
 470                 }
 471                 i += 1;
 472                 delete p;
 473                 p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 474         }
 475 }
 476
 477 BicycleCar *ParallelSlot::getEP()
 478 {
 479         // new pose for parallel parking to right slot
 480         float tnx;
 481         float tny;
 482         float nx;
 483         float ny;
 484         BicycleCar *CC = this->getEPC();
 485         // move left by car width / 2
 486         tnx = CC->x() + CC->width() / 2 * cos(CC->h() + M_PI / 2);
 487         tny = CC->y() + CC->width() / 2 * sin(CC->h() + M_PI / 2);
 488         if (this->slotSide() == LEFT) {
 489                 // move right by car width / 2
 490                 tnx = CC->x() + CC->width() / 2 * cos(CC->h() - M_PI / 2);
 491                 tny = CC->y() + CC->width() / 2 * sin(CC->h() - M_PI / 2);
 492         }
 493         if (this->slotType() == PARALLEL) {
 494                 // move down
 495                 nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
 496                 ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
 497         } else {
 498                 // move down
 499                 nx = tnx + (CC->length() - CC->wheelbase()) / 2 * cos(CC->h());
 500                 ny = tny + (CC->length() - CC->wheelbase()) / 2 * sin(CC->h());
 501         }
 502         return new BicycleCar(nx, ny, CC->h());
 503 }
 504
 505 BicycleCar *ParallelSlot::getEPC()
 506 {
 507         // new pose for parallel parking to right slot
 508         float ta;
 509         float nx;
 510         float ny;
 511         ta = this->slotHeading() + M_PI;
 512         if (this->slotSide() == RIGHT)
 513                 ta -= M_PI / 4;
 514         else
 515                 ta += M_PI / 4;
 516         nx = this->fposecenter()->x() + 0.01 * cos(ta);
 517         ny = this->fposecenter()->y() + 0.01 * sin(ta);
 518         return new BicycleCar(nx, ny, this->slotHeading());
 519 }
 520
 521 BicycleCar *ParallelSlot::getFP()
 522 {
 523         float x = this->slot().bnodes()[3]->x();
 524         float y = this->slot().bnodes()[3]->y();
 525         float h = this->slotHeading();
 526         float nx;
 527         float ny;
 528         if (this->slotType() == PARALLEL) {
 529                 if (this->slotSide() == LEFT) {
 530                         nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
 531                         ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
 532                 } else {
 533                         nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
 534                         ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
 535                 }
 536                 x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
 537                 y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
 538         } else {
 539                 if (this->slotSide() == LEFT) {
 540                         h -= M_PI / 2;
 541                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 542                                 * cos(h + M_PI);
 543                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 544                                 * sin(h + M_PI);
 545                         x = nx + (BCAR_DIAG_RRADI) * cos(h + M_PI / 2);
 546                         y = ny + (BCAR_DIAG_RRADI) * sin(h + M_PI / 2);
 547                 } else {
 548                         h += M_PI / 2;
 549                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 550                                 * cos(h - M_PI);
 551                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 552                                 * sin(h - M_PI);
 553                         x = nx + (BCAR_DIAG_RRADI) * cos(h - M_PI / 2);
 554                         y = ny + (BCAR_DIAG_RRADI) * sin(h - M_PI / 2);
 555                 }
 556         }
 557         return new BicycleCar(x, y, h);
 558 }
 559
 560 bool ParallelSlot::isInside(BicycleCar *c)
 561 {
 562         bool inside = true;
 563         RRTNode *tmpn;
 564         tmpn = new RRTNode(c->lfx(), c->lfy(), 0);
 565         if (!this->slot().collide(tmpn))
 566                 inside = false;
 567         delete tmpn;
 568         tmpn = new RRTNode(c->lrx(), c->lry(), 0);
 569         if (!this->slot().collide(tmpn))
 570                 inside = false;
 571         delete tmpn;
 572         tmpn = new RRTNode(c->rrx(), c->rry(), 0);
 573         if (!this->slot().collide(tmpn))
 574                 inside = false;
 575         delete tmpn;
 576         tmpn = new RRTNode(c->rfx(), c->rfy(), 0);
 577         if (!this->slot().collide(tmpn))
 578                 inside = false;
 579         delete tmpn;
 580         return inside;
 581 }
 582
 583 struct SamplingInfo ParallelSlot::getSamplingInfo()
 584 {
 585         struct SamplingInfo si;
 586         if (this->slotType() == PARALLEL) {
 587                 si.x = this->slot().bnodes().front()->x();
 588                 si.y = this->slot().bnodes().front()->y();
 589                 si.mr = BCAR_WIDTH / 2;
 590                 si.mmh = 0;
 591         } else {
 592                 si.x = this->slot().bnodes().back()->x();
 593                 si.x -= this->slot().bnodes().front()->x();
 594                 si.x /= 2;
 595                 si.x += this->slot().bnodes().front()->x();
 596                 si.y = this->slot().bnodes().back()->y();
 597                 si.y -= this->slot().bnodes().front()->y();
 598                 si.y /= 2;
 599                 si.y += this->slot().bnodes().front()->y();
 600                 si.mr = 0;
 601                 si.mmh = (M_PI - M_PI / 6) / 2;
 602         }
 603         si.r = BCAR_LENGTH;
 604         si.h = M_PI;
 605         si.mh = M_PI / 2;
 606         si.sh = this->slotHeading();
 607         if (this->slotSide() == RIGHT)
 608                 si.dh = 1;
 609         else
 610                 si.dh = -1;
 611         return si;
 612 }