Rename `possible_inits` to `possible_goals`

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

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 72aefa49cca54978ad8ad22c73530c11bd231ccb..c9278b6aa3f4add585b422bb737b0a74ba43a0d7 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -19,7 +19,7 @@ WVTEST_MAIN("parallel parking slot planner")
 
         // init orientation
         WVPASS(!psp.collide());
-        WVPASS(psp.forward());
+        WVPASS(!psp.forward());
         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
         WVPASS(psp.parked());
         std::vector<std::tuple<double, double>> slot;
@@ -126,3 +126,36 @@ WVTEST_MAIN("forward perpendicullar parking slot planner")
         WVPASS(psp.left());
         WVPASS(psp.parked());
 }
+
+WVTEST_MAIN("possible goals test")
+{
+        PSPlanner psp;
+        psp.gc().x(0);
+        psp.gc().y(0);
+        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());
+        psp.cc().sp(1);
+        psp.cc().st(0);
+        psp.cc().next();
+        WVPASSEQ_DOUBLE(psp.gc().x() + 1, psp.cc().x(), 0.00001);
+        WVPASSEQ_DOUBLE(psp.gc().y(), psp.cc().y(), 0.00001);
+        WVPASSEQ_DOUBLE(psp.gc().h(), psp.cc().h(), 0.00001);
+        WVPASS(psp.possible_goals().size() > 0);
+        double i = 1;
+        for (auto &c: psp.possible_goals()) {
+                WVPASSEQ_DOUBLE(psp.gc().x() + 1 + i, c.x(), 0.00001);
+                WVPASSEQ_DOUBLE(psp.gc().y(), c.y(), 0.00001);
+                WVPASSEQ_DOUBLE(psp.gc().h(), c.h(), 0.00001);
+                i++;
+        }
+        WVPASSEQ_DOUBLE(psp.gc().x() + 1, psp.cc().x(), 0.00001);
+        WVPASSEQ_DOUBLE(psp.gc().y(), psp.cc().y(), 0.00001);
+        WVPASSEQ_DOUBLE(psp.gc().h(), psp.cc().h(), 0.00001);
+}