Add ut for fe method

[hubacji1/psp.git] / ut / psp.t.cc

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index f2b5e500276fe9bb8c40f4714751f07bba15f444..47adc30ee273bbff62f6bc861942ac19d9c4c8df 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -21,6 +21,7 @@ WVTEST_MAIN("parallel parking slot planner")
 
         // init orientation
         WVPASS(!psp.collide());
+        WVPASS(psp.forward());
         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
 
         // entry point found by reverse
@@ -28,6 +29,12 @@ WVTEST_MAIN("parallel parking slot planner")
         psp.fer();
         WVPASS(psp.left());
 
+        // find entry
+        psp.cc() = BicycleCar(psp.gc());
+        WVPASS(!psp.left());
+        psp.fe();
+        WVPASS(psp.left());
+
         // collide
         auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
         WVPASS(std::get<0>(tmpi1));
@@ -63,12 +70,19 @@ WVTEST_MAIN("backward perpendicullar parking slot planner")
 
         // init orientation
         WVPASS(!psp.collide());
+        WVPASS(!psp.forward());
         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());
+
+        // find entry
+        psp.cc() = BicycleCar(psp.gc());
+        WVPASS(!psp.left());
+        psp.fe();
+        WVPASS(psp.left());
 }
 
 WVTEST_MAIN("forward perpendicullar parking slot planner")
@@ -89,10 +103,17 @@ WVTEST_MAIN("forward perpendicullar parking slot planner")
 
         // init orientation
         WVPASS(!psp.collide());
+        WVPASS(psp.forward());
         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());
+
+        // find entry
+        psp.cc() = BicycleCar(psp.gc());
+        WVPASS(!psp.left());
+        psp.fe();
+        WVPASS(psp.left());
 }