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 "bcar.h"
  19 #include "slotplanner.h"
  20
  21 ParallelSlot::ParallelSlot()
  22 {}
  23
  24 // getter
  25 std::vector<std::vector<RRTNode *>> &ParallelSlot::cusp()
  26 {
  27         return this->cusp_;
  28 }
  29
  30 float ParallelSlot::DH() const
  31 {
  32         return this->DH_;
  33 }
  34
  35 PolygonObstacle &ParallelSlot::slot()
  36 {
  37         return this->slot_;
  38 }
  39
  40 // setter
  41 void ParallelSlot::DH(float dh)
  42 {
  43         this->DH_ = dh;
  44 }
  45
  46 // other
  47 RRTNode *ParallelSlot::fposecenter()
  48 {
  49         if (this->slot().bnodes().front()->y() >
  50                         this->slot().bnodes().back()->y())
  51                 return this->slot().bnodes().front();
  52         else
  53                 return this->slot().bnodes().back();
  54 }
  55
  56 bool ParallelSlot::flast(RRTNode *P, bool right, int il)
  57 {
  58         RRTNode *cc;
  59         if (right)
  60                 cc = BicycleCar(
  61                         this->fposecenter()->x(),
  62                         this->fposecenter()->y(),
  63                         this->fposecenter()->h()
  64                 ).ccr();
  65         else
  66                 cc = BicycleCar(
  67                         this->fposecenter()->x(),
  68                         this->fposecenter()->y(),
  69                         this->fposecenter()->h()
  70                 ).ccl();
  71         BicycleCar *p;
  72         BicycleCar *B = new BicycleCar(P->x(), P->y(), P->h());
  73         int i = 1;
  74         p = B->move(cc, i * this->DH());
  75         while (!this->slot().collide(p->frame()) && p->x() <= 0) {
  76                 if (p->rfx() <= 0 && p->rrx() <= 0) {
  77                         i += 1;
  78                         break;
  79                 }
  80                 i += 1;
  81                 delete p;
  82                 p = B->move(cc, i * this->DH());
  83         }
  84         delete p;
  85         p = B->move(cc, (i - 1) * this->DH());
  86         if (p->rfx() <= 0 && p->rrx() <= 0) {
  87                 this->cusp().back().push_back(p);
  88                 return true;
  89         } else if (il < 8) {
  90                 this->cusp().back().push_back(p);
  91                 this->flast(p, !right, il + 1);
  92         }
  93         return false;
  94 }