Update test template

Include parking slot planner by default. This allows to input JSON
scenarios as intended -- with no goal position, only parking slot.

diff --git a/src/test-template.cc b/src/test-template.cc
index f3fda6bda63baaba75bc0c6b34ca5ff682c9448d..005fac30faf8375674a3ee4d1bc0657093bda0de 100644 (file)
--- a/src/test-template.cc
+++ b/src/test-template.cc
@@ -2,6 +2,7 @@
 #include <iostream>
 #include <jsoncpp/json/json.h>
 
+#include "psp.h"
 #include "rrtce.h"
 #define EPP RRTS
 
@@ -25,14 +26,48 @@ inline void TPRINT(const char *what)
 int main()
 {
         Json::Value jvi; // JSON input
+        Json::Value jvo; // JSON output
         std::cin >> jvi;
 
+        PSPlanner psp;
+        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.fe();
+
+        jvi["goal"][0] = psp.cc().x();
+        jvi["goal"][1] = psp.cc().y();
+        jvi["goal"][2] = psp.cc().h();
+        unsigned int icnt = 0;
+        for (auto &i: psp.possible_goals()) {
+                jvi["goals"][icnt][0] = i.x();
+                jvi["goals"][icnt][1] = i.y();
+                jvi["goals"][icnt][2] = i.h();
+                icnt++;
+        }
+        jvi["goals"][icnt][0] = psp.cc().x();
+        jvi["goals"][icnt][1] = psp.cc().y();
+        jvi["goals"][icnt][2] = psp.cc().h();
+
         EPP epp;
         epp.json(jvi);
         epp.init();
 
         while (epp.next()) {}
 
-        std::cout << epp.json() << std::endl;
+        jvo = epp.json();
+        jvo["slot"] = jvi["slot"];
+        std::cout << jvo << std::endl;
         return 0;
 }