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 "bcar.h"
  23 #include "slotplanner.h"
  24
  25 ParallelSlot::ParallelSlot()
  26 {}
  27
  28 // getter
  29 std::vector<std::vector<RRTNode *>> &ParallelSlot::cusp()
  30 {
  31         return this->cusp_;
  32 }
  33
  34 float ParallelSlot::DH() const
  35 {
  36         return this->DH_;
  37 }
  38
  39 PolygonObstacle &ParallelSlot::slot()
  40 {
  41         return this->slot_;
  42 }
  43
  44 // setter
  45 void ParallelSlot::DH(float dh)
  46 {
  47         this->DH_ = dh;
  48 }
  49
  50 // other
  51 void ParallelSlot::fip()
  52 {
  53         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
  54         // RRTNode.s() works as iteration level
  55         std::queue<BicycleCar *, std::list<BicycleCar *>> q;
  56
  57         // TODO add init nodes
  58         // for now just copy fpose()
  59         bool left = false; // right parking slot
  60         float di = -1;
  61         BicycleCar *CC = new BicycleCar(
  62                 this->fposecenter()->x(),
  63                 this->fposecenter()->y() - 0.01,
  64                 M_PI / 2
  65         );
  66         BicycleCar *B = new BicycleCar(
  67                 CC->x() - CC->width() / 2,
  68                 CC->y() - (CC->length() + CC->wheelbase()) / 2,
  69                 M_PI / 2
  70         );
  71         if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
  72                 left = true;
  73                 di = 1;
  74                 delete B;
  75                 B = new BicycleCar(
  76                         CC->x() + CC->width() / 2,
  77                         CC->y() - (CC->length() + CC->wheelbase()) / 2,
  78                         M_PI / 2
  79                 );
  80         }
  81         this->DH(di * 0.01 / CC->out_radi());
  82         BicycleCar *c;
  83         int i = 0;
  84         c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
  85         while (!this->slot().collide(c->frame())) {
  86                 q.push(c);
  87                 c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
  88                 i += 1;
  89         }
  90         delete c; // not in q and collide
  91         // BFS
  92         while (!q.empty()) {
  93                 c = q.front();
  94                 q.pop();
  95                 if (this->DH() > 0 && c->rfx() <= 0 && c->rrx() <= 0) {
  96                         goto createcuspandfinish;
  97                 } else if (this->DH() < 0 && c->lfx() >= 0 && c->lrx() >= 0) {
  98                         goto createcuspandfinish;
  99                 } else if (c->s() < 9) {
 100                         BicycleCar *cc = this->flnc(c);
 101                         cc->s(c->s() + 1);
 102                         cc->bcparent(c);
 103                         q.push(cc);
 104                 } else {
 105                         delete c; // not in q and collide
 106                 }
 107         }
 108 createcuspandfinish:
 109         std::vector<RRTNode *> cusp;
 110         while (c) {
 111                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 112                 c = c->bcparent();
 113         }
 114         std::reverse(cusp.begin(), cusp.end());
 115         this->cusp().push_back(cusp);
 116         std::queue<BicycleCar *, std::list<BicycleCar *>> empty;
 117         std::swap(q, empty);
 118 }
 119
 120 RRTNode *ParallelSlot::fposecenter()
 121 {
 122         if (this->slot().bnodes().front()->y() >
 123                         this->slot().bnodes().back()->y())
 124                 return this->slot().bnodes().front();
 125         else
 126                 return this->slot().bnodes().back();
 127 }
 128
 129 bool ParallelSlot::flast(
 130         RRTNode *P,
 131         bool right,
 132         int il,
 133         std::vector<RRTNode *> &cusp
 134 )
 135 {
 136         BicycleCar *B = new BicycleCar(P->x(), P->y(), P->h());
 137         RRTNode *cc;
 138         if (right)
 139                 cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 140         else
 141                 cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 142         BicycleCar *p;
 143         int i = 1;
 144         p = B->move(cc, i * this->DH());
 145         while (!this->slot().collide(p->frame())
 146                         && (
 147                                 (this->DH() > 0 && p->x() <= 0)
 148                                 || (this->DH() < 0 && p->x() >= 0)
 149                         )) {
 150                 if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 151                         i += 1;
 152                         break;
 153                 }
 154                 if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 155                         i += 1;
 156                         break;
 157                 }
 158                 i += 1;
 159                 delete p;
 160                 p = B->move(cc, i * this->DH());
 161         }
 162         delete p;
 163         p = B->move(cc, (i - 1) * this->DH());
 164         if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 165                 cusp.push_back(p);
 166                 return true;
 167         } else if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 168                 cusp.push_back(p);
 169                 return true;
 170         } else if (il < 8) {
 171                 cusp.push_back(p);
 172                 return this->flast(p, !right, il + 1, cusp);
 173         }
 174         return false;
 175 }
 176
 177 void ParallelSlot::fpose()
 178 {
 179         bool left = false; // right parking slot
 180         float di = -1;
 181         BicycleCar *CC = new BicycleCar(
 182                 this->fposecenter()->x(),
 183                 this->fposecenter()->y() - 0.01,
 184                 M_PI / 2
 185         );
 186         BicycleCar *B = new BicycleCar(
 187                 CC->x() - CC->width() / 2,
 188                 CC->y() - (CC->length() + CC->wheelbase()) / 2,
 189                 M_PI / 2
 190         );
 191         if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
 192                 left = true;
 193                 di = 1;
 194                 delete B;
 195                 B = new BicycleCar(
 196                         CC->x() + CC->width() / 2,
 197                         CC->y() - (CC->length() + CC->wheelbase()) / 2,
 198                         M_PI / 2
 199                 );
 200         }
 201         this->DH(di * 0.01 / CC->out_radi());
 202         BicycleCar *p;
 203         int i = 0;
 204         p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 205         while (!this->slot().collide(p->frame())) {
 206                 std::vector<RRTNode *> tmpcusp;
 207                 tmpcusp.push_back(new BicycleCar(p->x(), p->y(), p->h()));
 208                 if (this->flast(p, left, 0, tmpcusp)) {
 209                         this->cusp().push_back(tmpcusp);
 210                         return;
 211                 }
 212                 i += 1;
 213                 delete p;
 214                 p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 215         }
 216 }