Add init pose for forward perpendicular parking

[hubacji1/iamcar.git] / incl / slotplanner.h

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 23aa2b1da6cdeceb7eb1db55e5ad0b49cfff635b..ef539cd314b7a0d05e19a63786561a5a26e4951a 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -18,4 +18,113 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #ifndef SLOTPLANNER_H
 #define SLOTPLANNER_H
 
+#include <vector>
+#include "aux.h"
+#include "bcar.h"
+#include "obstacle.h"
+
+enum SlotSide {
+        LEFT,
+        RIGHT
+};
+
+enum SlotType {
+        PARALLEL,
+        PERPENDICULAR
+};
+
+struct SamplingInfo {
+        float x0;
+        float y0;
+        float h0;
+        float x;
+        float y;
+        float h;
+};
+
+class ParallelSlot {
+        private:
+                float DH_ = 0.01;
+                std::vector<std::vector<RRTNode *>> cusp_;
+                std::vector<RRTNode *> goals_;
+                PolygonObstacle slot_;
+                float slotHeading_;
+                SlotSide slotSide_;
+                SlotType slotType_;
+        public:
+                ParallelSlot();
+
+                // getter
+                /** Get slot entry point */
+                std::vector<RRTNode *> &goals();
+                RRTNode *getMidd();
+                std::vector<std::vector<RRTNode *>> &cusp();
+                float DH() const;
+                PolygonObstacle &slot();
+                float slotHeading();
+                SlotSide slotSide();
+                SlotType slotType();
+
+                // setter
+                void DH(float dh);
+                void setAll();
+
+                // other
+                /** BFS to _Find Init Pose_. */
+                void fip(
+                        std::vector<CircleObstacle>& co,
+                        std::vector<SegmentObstacle>& so
+                );
+                /** _Find Last Not Colliding_ BicycleCar pose
+
+                @param B Find from?
+                */
+                BicycleCar *flnc(
+                        BicycleCar *B,
+                        std::vector<CircleObstacle>& co,
+                        std::vector<SegmentObstacle>& so
+                );
+                /** _Find Init Pose by Reverse_ approach, see Vorobieva2015
+
+                @param B Last pose of vehicle when it is parked.
+                */
+                void fipr(RRTNode *n);
+                void fipr(BicycleCar *B);
+                /** _Find Last Not Colliding for Reverse_ BicycleCar pose
+
+                @param B Find from?
+                */
+                BicycleCar *flncr(BicycleCar *B);
+                RRTNode *fposecenter();
+                /** Recursive function to find out the moves */
+                bool flast(
+                        RRTNode *P,
+                        bool right,
+                        int il,
+                        std::vector<RRTNode *> &cusp
+                );
+                /** Test possible init poses */
+                void fpose();
+                /** Return start pose for fip method */
+                BicycleCar *getEP();
+                /** Return center of rotation for start pose for fip method */
+                BicycleCar *getEPC();
+                /** Return start pose for fipr method */
+                BicycleCar *getFP();
+                BicycleCar *getFPf(); // parked forward, only for perpendicular
+                /** In slot perpendicular pose getter
+
+                This method returns a pose of perpendicular parking slot from
+                where it is possible to get out of the slot with full steer to
+                farther corner side (i.e. right corner for the RIGHT side).
+
+                @param B The pose to start from.
+                */
+                BicycleCar *getISPP(BicycleCar *B);
+                /** Return true if car is inside slot */
+                bool isInside(BicycleCar *c);
+                /** Return values to set sampling function of RRT */
+                struct SamplingInfo getSamplingInfo();
+};
+
 #endif