From 4f5cf0b052b9a71e1553847f0d850e7f0d53793a Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 13 Jan 2020 13:23:31 +0100 Subject: [PATCH] Change test1 (parking slot planner) - Run both, fe and fer tests. - Test multiple slot lengths. - Output scenarios to stdout. - Output gnuplotable stats to stderr. --- src/test1.cc | 129 ++++++++++++++++++++++++++++----------------------- 1 file changed, 70 insertions(+), 59 deletions(-) diff --git a/src/test1.cc b/src/test1.cc index 7450c55..9ad0617 100644 --- a/src/test1.cc +++ b/src/test1.cc @@ -23,50 +23,40 @@ inline void TPRINT(const char *what) int main() { - Json::Value jvi; // JSON input Json::Value jvo; // JSON output - std::cin >> jvi; - if (jvi["slot"] == Json::nullValue) { - std::cerr << "I need `slot` in JSON input scenario"; - std::cerr << std::endl; - return 1; - } - - PSPlanner psp; - // use 1st slot (index 0) - psp.ps().border( - jvi["slot"][0][0][0].asDouble(), - jvi["slot"][0][0][1].asDouble(), - jvi["slot"][0][1][0].asDouble(), - jvi["slot"][0][1][1].asDouble(), - jvi["slot"][0][2][0].asDouble(), - jvi["slot"][0][2][1].asDouble(), - jvi["slot"][0][3][0].asDouble(), - jvi["slot"][0][3][1].asDouble() - ); - psp.guess_gc(); - psp.cc() = BicycleCar(psp.gc()); - TSTART(); - psp.fe(); - TEND(); - - { + double offset = -1.276994; + double delta = 0.1; + std::cerr << "# dist fe cusps fe time fer cusps fer time"; + std::cerr << std::endl; + for (int i = 0; i < 14; i++) { + jvo["goals"] = Json::nullValue; + // setup + PSPlanner psp; + double ppsl = 0.02 + psp.cc().perfect_parking_slot_len(); + double psl = ppsl + offset + i * delta; + psp.ps().border( + 0, 0, + 2.2, 0, + 2.2, 0 + psl, + 0, 0 + psl + ); + // run fe planner + TSTART(); + psp.fe(); // or fer() + TEND(); + std::cerr << (psl - psp.cc().l()); + std::cerr << " "; + std::cerr << psp.c(); + std::cerr << " "; + std::cerr << TDIFF(); + // json output jvo["time"] = TDIFF(); - } - { - jvo["init"][0] = jvi["init"][0]; - jvo["init"][1] = jvi["init"][1]; - jvo["init"][2] = jvi["init"][2]; - } - { jvo["goal"][0] = psp.gc().x(); jvo["goal"][1] = psp.gc().y(); jvo["goal"][2] = psp.gc().h(); - } - { unsigned int icnt = 0; - for (auto &i: psp.possible_goals(10, 50)) { + for (auto &i: psp.cusps()) { jvo["goals"][icnt][0] = i.x(); jvo["goals"][icnt][1] = i.y(); jvo["goals"][icnt][2] = i.h(); @@ -75,29 +65,50 @@ int main() jvo["goals"][icnt][0] = psp.cc().x(); jvo["goals"][icnt][1] = psp.cc().y(); jvo["goals"][icnt][2] = psp.cc().h(); - } - { - jvo["slot"][0][0][0] = psp.ps().x1(); - jvo["slot"][0][0][1] = psp.ps().y1(); - jvo["slot"][0][1][0] = psp.ps().x2(); - jvo["slot"][0][1][1] = psp.ps().y2(); - jvo["slot"][0][2][0] = psp.ps().x3(); - jvo["slot"][0][2][1] = psp.ps().y3(); - jvo["slot"][0][3][0] = psp.ps().x4(); - jvo["slot"][0][3][1] = psp.ps().y4(); - } - { - unsigned int ocnt = 0; - for (auto o: jvi["obst"]) { - unsigned int ccnt = 0; - for (auto c: o) { - jvo["obst"][ocnt][ccnt][0] = c[0]; - jvo["obst"][ocnt][ccnt][1] = c[1]; - ccnt++; - } - ocnt++; + jvo["slot"][0][0][0] = psp.ps().x1(); + jvo["slot"][0][0][1] = psp.ps().y1(); + jvo["slot"][0][1][0] = psp.ps().x2(); + jvo["slot"][0][1][1] = psp.ps().y2(); + jvo["slot"][0][2][0] = psp.ps().x3(); + jvo["slot"][0][2][1] = psp.ps().y3(); + jvo["slot"][0][3][0] = psp.ps().x4(); + jvo["slot"][0][3][1] = psp.ps().y4(); + jvo["cusp"] = psp.c(); + std::cout << jvo << std::endl; + // run fer planner + TSTART(); + psp.fer(); // or fer() + TEND(); + std::cerr << " "; + std::cerr << psp.c(); + std::cerr << " "; + std::cerr << TDIFF(); + std::cerr << std::endl; + // json output + jvo["time"] = TDIFF(); + jvo["goal"][0] = psp.gc().x(); + jvo["goal"][1] = psp.gc().y(); + jvo["goal"][2] = psp.gc().h(); + icnt = 0; + for (auto &i: psp.cusps()) { + jvo["goals"][icnt][0] = i.x(); + jvo["goals"][icnt][1] = i.y(); + jvo["goals"][icnt][2] = i.h(); + icnt++; } + jvo["goals"][icnt][0] = psp.cc().x(); + jvo["goals"][icnt][1] = psp.cc().y(); + jvo["goals"][icnt][2] = psp.cc().h(); + jvo["slot"][0][0][0] = psp.ps().x1(); + jvo["slot"][0][0][1] = psp.ps().y1(); + jvo["slot"][0][1][0] = psp.ps().x2(); + jvo["slot"][0][1][1] = psp.ps().y2(); + jvo["slot"][0][2][0] = psp.ps().x3(); + jvo["slot"][0][2][1] = psp.ps().y3(); + jvo["slot"][0][3][0] = psp.ps().x4(); + jvo["slot"][0][3][1] = psp.ps().y4(); + jvo["cusp"] = psp.c(); + std::cout << jvo << std::endl; } - std::cout << jvo << std::endl; return 0; } -- 2.39.2