#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().x(4);
- psp.gc().y(4);
- psp.gc().h(M_PI / 2);
psp.gc().mtr(10);
psp.gc().wb(2);
psp.gc().w(1);
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));
- // entry point found by reverse
- WVPASS(!psp.left());
- psp.fer();
- WVPASS(psp.left());
-
- // collide
- auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
- WVPASS(std::get<0>(tmpi1));
- WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
- WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
- auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
- WVPASS(!std::get<0>(tmpi2));
+ // init collide
PSPlanner tpsp;
tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
tpsp.gc() = BicycleCar(psp.gc());
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(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());
+ 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);
}