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         auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
  40         WVPASS(std::get<0>(tmpi1));
  41         WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
  42         WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
  43         auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
  44         WVPASS(!std::get<0>(tmpi2));
  45         PSPlanner tpsp;
  46         tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
  47         tpsp.gc() = BicycleCar(psp.gc());
  48         tpsp.cc() = BicycleCar(tpsp.gc());
  49         WVPASS(tpsp.collide());
  50         WVPASS(!tpsp.left());
  51         tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
  52         WVPASS(tpsp.left());
  53
  54         // entry point found by reverse
  55         WVPASS(!psp.left());
  56         psp.fer();
  57         WVPASS(psp.left());
  58
  59         // find entry
  60         psp.cc() = BicycleCar(psp.gc());
  61         WVPASS(!psp.left());
  62         psp.fe();
  63         WVPASS(psp.left());
  64 }
  65
  66 WVTEST_MAIN("backward perpendicullar parking slot planner")
  67 {
  68         PSPlanner psp;
  69         psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
  70         psp.gc().x(7);
  71         psp.gc().y(4);
  72         psp.gc().h(M_PI);
  73         psp.gc().mtr(10);
  74         psp.gc().wb(2);
  75         psp.gc().w(1);
  76         psp.gc().l(3);
  77         psp.gc().he(1.5);
  78         psp.gc().df(2 + 0.5);
  79         psp.gc().dr(0.5);
  80         psp.cc() = BicycleCar(psp.gc());
  81
  82         // init orientation
  83         WVPASS(!psp.collide());
  84         WVPASS(!psp.forward());
  85         WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
  86         WVPASS(psp.parked());
  87
  88         // entry point found by reverse
  89         WVPASS(!psp.left());
  90         psp.fer();
  91         WVPASS(psp.left());
  92
  93         // find entry
  94         psp.cc() = BicycleCar(psp.gc());
  95         WVPASS(!psp.left());
  96         psp.fe();
  97         WVPASS(psp.left());
  98 }
  99
 100 WVTEST_MAIN("forward perpendicullar parking slot planner")
 101 {
 102         PSPlanner psp;
 103         psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
 104         psp.gc().x(4);
 105         psp.gc().y(4);
 106         psp.gc().h(0);
 107         psp.gc().mtr(10);
 108         psp.gc().wb(2);
 109         psp.gc().w(1);
 110         psp.gc().l(3);
 111         psp.gc().he(1.5);
 112         psp.gc().df(2 + 0.5);
 113         psp.gc().dr(0.5);
 114         psp.cc() = BicycleCar(psp.gc());
 115
 116         // init orientation
 117         WVPASS(!psp.collide());
 118         WVPASS(psp.forward());
 119         WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
 120         WVPASS(psp.parked());
 121
 122         // entry point found by reverse
 123         WVPASS(!psp.left());
 124         psp.fer();
 125         WVPASS(psp.left());
 126
 127         // find entry
 128         psp.cc() = BicycleCar(psp.gc());
 129         WVPASS(!psp.left());
 130         psp.fe();
 131         WVPASS(psp.left());
 132 }