ut/psp.t.cc

   1 #include <cmath>
   2 #include "wvtest.h"
   3
   4 #include "psp.h"
   5
   6 WVTEST_MAIN("parallel parking slot planner")
   7 {
   8         PSPlanner psp;
   9         psp.ps().border(3, 3, 5, 3, 5, 8, 3, 8);
  10         psp.gc().mtr(10);
  11         psp.gc().wb(2);
  12         psp.gc().w(1);
  13         psp.gc().l(3);
  14         psp.gc().he(1.5);
  15         psp.gc().df(2 + 0.5);
  16         psp.gc().dr(0.5);
  17         psp.guess_gc();
  18         psp.cc() = BicycleCar(psp.gc());
  19
  20         // init orientation
  21         WVPASS(!psp.collide());
  22         WVPASS(!psp.forward());
  23         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
  24         WVPASS(psp.parked());
  25         std::vector<std::tuple<double, double>> slot;
  26         slot.push_back(std::make_tuple(psp.ps().x1(), psp.ps().y1()));
  27         slot.push_back(std::make_tuple(psp.ps().x2(), psp.ps().y2()));
  28         slot.push_back(std::make_tuple(psp.ps().x3(), psp.ps().y3()));
  29         slot.push_back(std::make_tuple(psp.ps().x4(), psp.ps().y4()));
  30         WVPASS(inside(psp.gc().x(), psp.gc().y(), slot));
  31         WVPASS(inside(psp.gc().lfx(), psp.gc().lfy(), slot));
  32         WVPASS(inside(psp.gc().lrx(), psp.gc().lry(), slot));
  33         WVPASS(inside(psp.gc().rrx(), psp.gc().rry(), slot));
  34         WVPASS(inside(psp.gc().rfx(), psp.gc().rfy(), slot));
  35
  36         // init collide
  37         PSPlanner tpsp;
  38         tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
  39         tpsp.gc() = BicycleCar(psp.gc());
  40         tpsp.cc() = BicycleCar(tpsp.gc());
  41         WVPASS(tpsp.collide());
  42         WVPASS(!tpsp.left());
  43         tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
  44         WVPASS(tpsp.left());
  45
  46         // entry point found by reverse
  47         WVPASS(!psp.left());
  48         psp.fer();
  49         WVPASS(psp.left());
  50         WVPASS(psp.parked());
  51
  52         // find entry
  53         psp.cc() = BicycleCar(psp.gc());
  54         WVPASS(!psp.left());
  55         psp.fe();
  56         WVPASS(psp.left());
  57         WVPASS(psp.parked());
  58 }
  59
  60 WVTEST_MAIN("backward perpendicullar parking slot planner")
  61 {
  62         PSPlanner psp;
  63         psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
  64         psp.gc().mtr(10);
  65         psp.gc().wb(2);
  66         psp.gc().w(1);
  67         psp.gc().l(3);
  68         psp.gc().he(1.5);
  69         psp.gc().df(2 + 0.5);
  70         psp.gc().dr(0.5);
  71         psp.guess_gc();
  72         psp.cc() = BicycleCar(psp.gc());
  73
  74         // init orientation
  75         WVPASS(!psp.collide());
  76         WVPASS(!psp.forward());
  77         WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
  78         WVPASS(psp.parked());
  79
  80         // entry point found by reverse
  81         WVPASS(!psp.left());
  82         psp.fer();
  83         WVPASS(psp.left());
  84         WVPASS(psp.parked());
  85
  86         // find entry
  87         psp.cc() = BicycleCar(psp.gc());
  88         WVPASS(!psp.left());
  89         psp.fe();
  90         WVPASS(psp.left());
  91         WVPASS(psp.parked());
  92 }
  93
  94 WVTEST_MAIN("forward perpendicullar parking slot planner")
  95 {
  96         PSPlanner psp;
  97         psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
  98         psp.gc().x(4);
  99         psp.gc().y(4);
 100         psp.gc().h(0);
 101         psp.gc().mtr(10);
 102         psp.gc().wb(2);
 103         psp.gc().w(1);
 104         psp.gc().l(3);
 105         psp.gc().he(1.5);
 106         psp.gc().df(2 + 0.5);
 107         psp.gc().dr(0.5);
 108         psp.cc() = BicycleCar(psp.gc());
 109
 110         // init orientation
 111         WVPASS(!psp.collide());
 112         WVPASS(psp.forward());
 113         WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
 114         WVPASS(psp.parked());
 115
 116         // entry point found by reverse
 117         WVPASS(!psp.left());
 118         psp.fer();
 119         WVPASS(psp.left());
 120         WVPASS(psp.parked());
 121
 122         // find entry
 123         psp.cc() = BicycleCar(psp.gc());
 124         WVPASS(!psp.left());
 125         psp.fe();
 126         WVPASS(psp.left());
 127         WVPASS(psp.parked());
 128 }
 129
 130 WVTEST_MAIN("possible goals test")
 131 {
 132         PSPlanner psp;
 133         psp.gc().x(0);
 134         psp.gc().y(0);
 135         psp.gc().h(0);
 136         psp.gc().mtr(10);
 137         psp.gc().wb(2);
 138         psp.gc().w(1);
 139         psp.gc().l(3);
 140         psp.gc().he(1.5);
 141         psp.gc().df(2 + 0.5);
 142         psp.gc().dr(0.5);
 143         psp.cc() = BicycleCar(psp.gc());
 144         psp.cc().sp(1);
 145         psp.cc().st(0);
 146         psp.cc().next();
 147         WVPASSEQ_DOUBLE(psp.gc().x() + 1, psp.cc().x(), 0.00001);
 148         WVPASSEQ_DOUBLE(psp.gc().y(), psp.cc().y(), 0.00001);
 149         WVPASSEQ_DOUBLE(psp.gc().h(), psp.cc().h(), 0.00001);
 150         WVPASS(psp.possible_goals().size() > 0);
 151         double i = 1;
 152         for (auto &c: psp.possible_goals()) {
 153                 WVPASSEQ_DOUBLE(psp.gc().x() + 1 + i, c.x(), 0.00001);
 154                 WVPASSEQ_DOUBLE(psp.gc().y(), c.y(), 0.00001);
 155                 WVPASSEQ_DOUBLE(psp.gc().h(), c.h(), 0.00001);
 156                 i++;
 157         }
 158         WVPASSEQ_DOUBLE(psp.gc().x() + 1, psp.cc().x(), 0.00001);
 159         WVPASSEQ_DOUBLE(psp.gc().y(), psp.cc().y(), 0.00001);
 160         WVPASSEQ_DOUBLE(psp.gc().h(), psp.cc().h(), 0.00001);
 161 }