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 x;
  38         float y;
  39         float r; // max radi from [x, y] for random sample
  40         float mr; // minimum r added to random r
  41         float h; // max angle for random sample
  42         float mh; // max angle for heading of random sample
  43         float mmh; // minimum mh added to mh
  44         float sh; // slot heading
  45         float dh; // direction to compute random heading from slot heading
  46 };
  47
  48 class ParallelSlot {
  49         private:
  50                 float DH_ = 0.01;
  51                 std::vector<std::vector<RRTNode *>> cusp_;
  52                 PolygonObstacle slot_;
  53                 float slotHeading_;
  54                 SlotSide slotSide_;
  55                 SlotType slotType_;
  56         public:
  57                 ParallelSlot();
  58
  59                 // getter
  60                 /** Get slot entry point */
  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
  69                 // setter
  70                 void DH(float dh);
  71                 void setAll();
  72
  73                 // other
  74                 /** BFS to _Find Init Pose_. */
  75                 void fip(
  76                         std::vector<CircleObstacle>& co,
  77                         std::vector<SegmentObstacle>& so
  78                 );
  79                 /** _Find Last Not Colliding_ BicycleCar pose
  80
  81                 @param B Find from?
  82                 */
  83                 BicycleCar *flnc(
  84                         BicycleCar *B,
  85                         std::vector<CircleObstacle>& co,
  86                         std::vector<SegmentObstacle>& so
  87                 );
  88                 /** _Find Init Pose by Reverse_ approach, see Vorobieva2015
  89
  90                 @param B Last pose of vehicle when it is parked.
  91                 */
  92                 void fipr(RRTNode *n);
  93                 void fipr(BicycleCar *B);
  94                 /** _Find Last Not Colliding for Reverse_ BicycleCar pose
  95
  96                 @param B Find from?
  97                 */
  98                 BicycleCar *flncr(BicycleCar *B);
  99                 RRTNode *fposecenter();
 100                 /** Recursive function to find out the moves */
 101                 bool flast(
 102                         RRTNode *P,
 103                         bool right,
 104                         int il,
 105                         std::vector<RRTNode *> &cusp
 106                 );
 107                 /** Test possible init poses */
 108                 void fpose();
 109                 /** Return start pose for fip method */
 110                 BicycleCar *getEP();
 111                 /** Return center of rotation for start pose for fip method */
 112                 BicycleCar *getEPC();
 113                 /** Return start pose for fipr method */
 114                 BicycleCar *getFP();
 115                 /** Return true if car is inside slot */
 116                 bool isInside(BicycleCar *c);
 117                 /** Return values to set sampling function of RRT */
 118                 struct SamplingInfo getSamplingInfo();
 119 };
 120
 121 #endif