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