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         std::queue<RRTNode *, std::list<RRTNode *>> q;
  53 }
  54
  55 RRTNode *ParallelSlot::fposecenter()
  56 {
  57         if (this->slot().bnodes().front()->y() >
  58                         this->slot().bnodes().back()->y())
  59                 return this->slot().bnodes().front();
  60         else
  61                 return this->slot().bnodes().back();
  62 }
  63
  64 bool ParallelSlot::flast(
  65         RRTNode *P,
  66         bool right,
  67         int il,
  68         std::vector<RRTNode *> &cusp
  69 )
  70 {
  71         BicycleCar *B = new BicycleCar(P->x(), P->y(), P->h());
  72         RRTNode *cc;
  73         if (right)
  74                 cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
  75         else
  76                 cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
  77         BicycleCar *p;
  78         int i = 1;
  79         p = B->move(cc, i * this->DH());
  80         while (!this->slot().collide(p->frame())
  81                         && (
  82                                 (this->DH() > 0 && p->x() <= 0)
  83                                 || (this->DH() < 0 && p->x() >= 0)
  84                         )) {
  85                 if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
  86                         i += 1;
  87                         break;
  88                 }
  89                 if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
  90                         i += 1;
  91                         break;
  92                 }
  93                 i += 1;
  94                 delete p;
  95                 p = B->move(cc, i * this->DH());
  96         }
  97         delete p;
  98         p = B->move(cc, (i - 1) * this->DH());
  99         if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 100                 cusp.push_back(p);
 101                 return true;
 102         } else if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 103                 cusp.push_back(p);
 104                 return true;
 105         } else if (il < 8) {
 106                 cusp.push_back(p);
 107                 return this->flast(p, !right, il + 1, cusp);
 108         }
 109         return false;
 110 }
 111
 112 void ParallelSlot::fpose()
 113 {
 114         bool left = false; // right parking slot
 115         float di = -1;
 116         BicycleCar *CC = new BicycleCar(
 117                 this->fposecenter()->x(),
 118                 this->fposecenter()->y() - 0.01,
 119                 M_PI / 2
 120         );
 121         BicycleCar *B = new BicycleCar(
 122                 CC->x() - CC->width() / 2,
 123                 CC->y() - (CC->length() + CC->wheelbase()) / 2,
 124                 M_PI / 2
 125         );
 126         if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
 127                 left = true;
 128                 di = 1;
 129                 delete B;
 130                 B = new BicycleCar(
 131                         CC->x() + CC->width() / 2,
 132                         CC->y() - (CC->length() + CC->wheelbase()) / 2,
 133                         M_PI / 2
 134                 );
 135         }
 136         this->DH(di * 0.01 / CC->out_radi());
 137         BicycleCar *p;
 138         int i = 0;
 139         p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 140         while (!this->slot().collide(p->frame())) {
 141                 std::vector<RRTNode *> tmpcusp;
 142                 tmpcusp.push_back(new BicycleCar(p->x(), p->y(), p->h()));
 143                 if (this->flast(p, left, 0, tmpcusp)) {
 144                         this->cusp().push_back(tmpcusp);
 145                         return;
 146                 }
 147                 i += 1;
 148                 delete p;
 149                 p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 150         }
 151 }