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 "bcar.h"
  23 #include "obstacle.h"
  24
  25 #define EDIST(a, b) ({ __typeof__ (a) _a = (a); \
  26                 __typeof__ (b) _b = (b); \
  27                 pow(pow((_b)->x() - (_a)->x(), 2) + \
  28                                 pow((_b)->y() - (_a)->y(), 2), 0.5); })
  29
  30 enum SlotType {
  31         PARALLEL,
  32         PERPENDICULAR
  33 };
  34
  35 struct SamplingInfo {
  36         float x;
  37         float y;
  38         float r;
  39         float h;
  40 };
  41
  42 class ParallelSlot {
  43         private:
  44                 float DH_ = 0.01;
  45                 std::vector<std::vector<RRTNode *>> cusp_;
  46                 PolygonObstacle slot_;
  47                 SlotType slotType_;
  48         public:
  49                 ParallelSlot();
  50
  51                 // getter
  52                 std::vector<std::vector<RRTNode *>> &cusp();
  53                 float DH() const;
  54                 PolygonObstacle &slot();
  55                 SlotType slotType();
  56
  57                 // setter
  58                 void DH(float dh);
  59
  60                 // other
  61                 /** BFS to _Find Init Pose_. */
  62                 void fip();
  63                 /** _Find Init Pose by Reverse_ approach, see Vorobieva2015
  64
  65                 @param B Last pose of vehicle when it is parked.
  66                 */
  67                 void fipr(BicycleCar *B);
  68                 /** _Find Last Not Colliding for Reverse_ BicycleCar pose
  69
  70                 @param B Find from?
  71                 */
  72                 BicycleCar *flncr(BicycleCar *B);
  73                 /** _Find Last Not Colliding_ BicycleCar pose
  74
  75                 @param B Find from?
  76                 */
  77                 BicycleCar *flnc(BicycleCar *B);
  78                 RRTNode *fposecenter();
  79                 /** Recursive function to find out the moves */
  80                 bool flast(
  81                         RRTNode *P,
  82                         bool right,
  83                         int il,
  84                         std::vector<RRTNode *> &cusp
  85                 );
  86                 /** Test possible init poses */
  87                 void fpose();
  88                 /** Return values to set sampling function of RRT */
  89                 struct SamplingInfo getSamplingInfo();
  90 };
  91
  92 #endif