incl/slotplanner.h

   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 #ifndef SLOTPLANNER_H
  19 #define SLOTPLANNER_H
  20
  21 #include <vector>
  22 #include "aux.h"
  23 #include "bcar.h"
  24 #include "obstacle.h"
  25
  26 enum SlotSide {
  27         LEFT,
  28         RIGHT
  29 };
  30
  31 enum SlotType {
  32         PARALLEL,
  33         PERPENDICULAR
  34 };
  35
  36 struct SamplingInfo {
  37         float x0;
  38         float y0;
  39         float h0;
  40         float x;
  41         float y;
  42         float h;
  43 };
  44
  45 class ParallelSlot {
  46         private:
  47                 float DH_ = 0.01;
  48                 std::vector<std::vector<RRTNode *>> cusp_;
  49                 std::vector<RRTNode *> goals_;
  50                 PolygonObstacle slot_;
  51                 float slotHeading_;
  52                 SlotSide slotSide_;
  53                 SlotType slotType_;
  54                 float poseHeading_;
  55         public:
  56                 ParallelSlot();
  57
  58                 // getter
  59                 /** Get slot entry point */
  60                 std::vector<RRTNode *> &goals();
  61                 RRTNode *getMidd();
  62                 std::vector<std::vector<RRTNode *>> &cusp();
  63                 float DH() const;
  64                 PolygonObstacle &slot();
  65                 float slotHeading();
  66                 SlotSide slotSide();
  67                 SlotType slotType();
  68                 float poseHeading();
  69
  70                 // setter
  71                 void DH(float dh);
  72                 void setAll();
  73
  74                 // other
  75                 /** BFS to _Find Init Pose_. */
  76                 // backward parking
  77                 void fip(
  78                         std::vector<CircleObstacle>& co,
  79                         std::vector<SegmentObstacle>& so
  80                 );
  81                 // forward parking
  82                 void fipf(
  83                         std::vector<CircleObstacle>& co,
  84                         std::vector<SegmentObstacle>& so
  85                 ); // perpendicular forward parking
  86                 /** _Find Last Not Colliding_ BicycleCar pose
  87
  88                 @param B Find from?
  89                 */
  90                 BicycleCar *flnc(
  91                         BicycleCar *B,
  92                         std::vector<CircleObstacle>& co,
  93                         std::vector<SegmentObstacle>& so
  94                 );
  95                 /** _Find Init Pose by Reverse_ approach, see Vorobieva2015
  96
  97                 @param B Last pose of vehicle when it is parked.
  98                 */
  99                 void fipr(RRTNode *n);
 100                 void fipr(BicycleCar *B);
 101                 /** _Find Last Not Colliding for Reverse_ BicycleCar pose
 102
 103                 @param B Find from?
 104                 */
 105                 BicycleCar *flncr(BicycleCar *B);
 106                 RRTNode *fposecenter();
 107                 /** Recursive function to find out the moves */
 108                 bool flast(
 109                         RRTNode *P,
 110                         bool right,
 111                         int il,
 112                         std::vector<RRTNode *> &cusp
 113                 );
 114                 /** Test possible init poses */
 115                 void fpose();
 116                 /** Return start pose for fip method */
 117                 BicycleCar *getEP();
 118                 /** Return center of rotation for start pose for fip method */
 119                 BicycleCar *getEPC();
 120                 /** Return start pose for fipr method */
 121                 BicycleCar *getFP();
 122                 BicycleCar *getFPf(); // parked forward, only for perpendicular
 123                 /** In slot perpendicular pose getter
 124
 125                 This method returns a pose of perpendicular parking slot from
 126                 where it is possible to get out of the slot with full steer to
 127                 farther corner side (i.e. right corner for the RIGHT side).
 128
 129                 @param B The pose to start from.
 130                 */
 131                 BicycleCar *getISPP(BicycleCar *B);
 132                 BicycleCar *getISPPf(BicycleCar *B); // perp. forward parking
 133                 /** Return true if car is inside slot */
 134                 bool isInside(BicycleCar *c);
 135                 /** Return values to set sampling function of RRT */
 136                 struct SamplingInfo getSamplingInfo();
 137 };
 138
 139 #endif