Add pose heading method to SlotPlanner

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

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 5da80770f2f63e2f5504c867ba1daf2e214ca445..4fc77de942ce598ad606620aaa70762777822e32 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -19,14 +19,10 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define SLOTPLANNER_H
 
 #include <vector>
+#include "aux.h"
 #include "bcar.h"
 #include "obstacle.h"
 
-#define EDIST(a, b) ({ __typeof__ (a) _a = (a); \
-                __typeof__ (b) _b = (b); \
-                pow(pow((_b)->x() - (_a)->x(), 2) + \
-                                pow((_b)->y() - (_a)->y(), 2), 0.5); })
-
 enum SlotSide {
         LEFT,
         RIGHT
@@ -38,9 +34,11 @@ enum SlotType {
 };
 
 struct SamplingInfo {
+        float x0;
+        float y0;
+        float h0;
         float x;
         float y;
-        float r;
         float h;
 };
 
@@ -48,41 +46,61 @@ 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_;
+                float poseHeading_;
         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();
+                SlotSide slotSide();
+                SlotType slotType();
+                float poseHeading();
 
                 // setter
                 void DH(float dh);
+                void setAll();
 
                 // other
                 /** BFS to _Find Init Pose_. */
-                void fip();
+                void fip(
+                        std::vector<CircleObstacle>& co,
+                        std::vector<SegmentObstacle>& so
+                );
+                void fipf(
+                        std::vector<CircleObstacle>& co,
+                        std::vector<SegmentObstacle>& so
+                ); // perpendicular forward parking
+                /** _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);
-                /** _Find Last Not Colliding_ BicycleCar pose
-
-                @param B Find from?
-                */
-                BicycleCar *flnc(BicycleCar *B);
                 RRTNode *fposecenter();
                 /** Recursive function to find out the moves */
                 bool flast(
@@ -93,6 +111,25 @@ class ParallelSlot {
                 );
                 /** 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);
+                BicycleCar *getISPPf(BicycleCar *B); // perp. forward parking
+                /** Return true if car is inside slot */
+                bool isInside(BicycleCar *c);
                 /** Return values to set sampling function of RRT */
                 struct SamplingInfo getSamplingInfo();
 };