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