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 7c82b110f283d215004cb750256a755a6393d7c5..c9278b6aa3f4add585b422bb737b0a74ba43a0d7 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -3,13 +3,101 @@
 
 #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);
+        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.guess_gc();
+        psp.cc() = BicycleCar(psp.gc());
+
+        // init orientation
+        WVPASS(!psp.collide());
+        WVPASS(!psp.forward());
+        WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
+        WVPASS(psp.parked());
+        std::vector<std::tuple<double, double>> slot;
+        slot.push_back(std::make_tuple(psp.ps().x1(), psp.ps().y1()));
+        slot.push_back(std::make_tuple(psp.ps().x2(), psp.ps().y2()));
+        slot.push_back(std::make_tuple(psp.ps().x3(), psp.ps().y3()));
+        slot.push_back(std::make_tuple(psp.ps().x4(), psp.ps().y4()));
+        WVPASS(inside(psp.gc().x(), psp.gc().y(), slot));
+        WVPASS(inside(psp.gc().lfx(), psp.gc().lfy(), slot));
+        WVPASS(inside(psp.gc().lrx(), psp.gc().lry(), slot));
+        WVPASS(inside(psp.gc().rrx(), psp.gc().rry(), slot));
+        WVPASS(inside(psp.gc().rfx(), psp.gc().rfy(), slot));
+
+        // init collide
+        PSPlanner tpsp;
+        tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
+        tpsp.gc() = BicycleCar(psp.gc());
+        tpsp.cc() = BicycleCar(tpsp.gc());
+        WVPASS(tpsp.collide());
+        WVPASS(!tpsp.left());
+        tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
+        WVPASS(tpsp.left());
+
+        // entry point found by reverse
+        WVPASS(!psp.left());
+        psp.fer();
+        WVPASS(psp.left());
+        WVPASS(psp.parked());
+
+        // find entry
+        psp.cc() = BicycleCar(psp.gc());
+        WVPASS(!psp.left());
+        psp.fe();
+        WVPASS(psp.left());
+        WVPASS(psp.parked());
+}
+
+WVTEST_MAIN("backward perpendicullar parking slot planner")
+{
+        PSPlanner psp;
+        psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
+        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.guess_gc();
+        psp.cc() = BicycleCar(psp.gc());
+
+        // init orientation
+        WVPASS(!psp.collide());
+        WVPASS(!psp.forward());
+        WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
+        WVPASS(psp.parked());
+
+        // entry point found by reverse
+        WVPASS(!psp.left());
+        psp.fer();
+        WVPASS(psp.left());
+        WVPASS(psp.parked());
+
+        // find entry
+        psp.cc() = BicycleCar(psp.gc());
+        WVPASS(!psp.left());
+        psp.fe();
+        WVPASS(psp.left());
+        WVPASS(psp.parked());
+}
+
+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(M_PI / 2);
+        psp.gc().h(0);
         psp.gc().mtr(10);
         psp.gc().wb(2);
         psp.gc().w(1);
@@ -17,7 +105,57 @@ WVTEST_MAIN("parking slot planner basic test")
         psp.gc().he(1.5);
         psp.gc().df(2 + 0.5);
         psp.gc().dr(0.5);
+        psp.cc() = BicycleCar(psp.gc());
 
         // init orientation
-        WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
+        WVPASS(!psp.collide());
+        WVPASS(psp.forward());
+        WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
+        WVPASS(psp.parked());
+
+        // entry point found by reverse
+        WVPASS(!psp.left());
+        psp.fer();
+        WVPASS(psp.left());
+        WVPASS(psp.parked());
+
+        // find entry
+        psp.cc() = BicycleCar(psp.gc());
+        WVPASS(!psp.left());
+        psp.fe();
+        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);
 }