Change test1 (parking slot planner)

[hubacji1/iamcar2.git] / src / test1.cc

diff --git a/src/test1.cc b/src/test1.cc
index b37a9ad6e46c22d52ae8d367a1e705beaef4b278..9ad06170fc1690e8a60075e049285450df85b0b3 100644 (file)
--- a/src/test1.cc
+++ b/src/test1.cc
@@ -23,66 +23,92 @@ 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] = psp.cc().x();
-                jvo["init"][1] = psp.cc().y();
-                jvo["init"][2] = psp.cc().h();
-        }
-        {
                 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_inits(10, 10)) {
-                        jvo["inits"][icnt][0] = i.x();
-                        jvo["inits"][icnt][1] = i.y();
-                        jvo["inits"][icnt][2] = i.h();
+                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;
+                // 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;
         }
-        {
-               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();
-        }
-        std::cout << jvo << std::endl;
         return 0;
 }