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