Add ut for perpendicullar fer method

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 94e8ccabed8b103182029814bb9294f855afef4d..f2b5e500276fe9bb8c40f4714751f07bba15f444 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -3,7 +3,7 @@
 
 #include "psp.h"
 
-WVTEST_MAIN("parking slot planner basic test")
+WVTEST_MAIN("parallel parking slot planner")
 {
         PSPlanner psp;
         psp.ps().border(3, 3, 5, 3, 5, 8, 3, 8);
@@ -44,3 +44,55 @@ WVTEST_MAIN("parking slot planner basic test")
         tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
         WVPASS(tpsp.left());
 }
+
+WVTEST_MAIN("backward perpendicullar parking slot planner")
+{
+        PSPlanner psp;
+        psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
+        psp.gc().x(7);
+        psp.gc().y(4);
+        psp.gc().h(M_PI);
+        psp.gc().mtr(10);
+        psp.gc().wb(2);
+        psp.gc().w(1);
+        psp.gc().l(3);
+        psp.gc().he(1.5);
+        psp.gc().df(2 + 0.5);
+        psp.gc().dr(0.5);
+        psp.cc() = BicycleCar(psp.gc());
+
+        // init orientation
+        WVPASS(!psp.collide());
+        WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
+
+        // entry point found by reverse
+        WVPASS(!psp.left());
+        psp.fer();
+        WVPASS(psp.left());
+}
+
+WVTEST_MAIN("forward perpendicullar parking slot planner")
+{
+        PSPlanner psp;
+        psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
+        psp.gc().x(4);
+        psp.gc().y(4);
+        psp.gc().h(0);
+        psp.gc().mtr(10);
+        psp.gc().wb(2);
+        psp.gc().w(1);
+        psp.gc().l(3);
+        psp.gc().he(1.5);
+        psp.gc().df(2 + 0.5);
+        psp.gc().dr(0.5);
+        psp.cc() = BicycleCar(psp.gc());
+
+        // init orientation
+        WVPASS(!psp.collide());
+        WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
+
+        // entry point found by reverse
+        WVPASS(!psp.left());
+        psp.fer();
+        WVPASS(psp.left());
+}