Output JSON goals as RRT found goal

[hubacji1/iamcar.git] / base / main.cc

diff --git a/base/main.cc b/base/main.cc
index 2e8f7cbb46446cc81d659a8867aaf40ee876c634..77ab2e1bffdf69059c57ef237c9a3e1d29c62fde 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -165,6 +165,9 @@ int main()
         jvo["init"][0] = p.root()->x();
         jvo["init"][1] = p.root()->y();
         jvo["init"][2] = p.root()->h();
+        jvo["goal"][0] = jvi["goal"][0].asFloat();
+        jvo["goal"][1] = jvi["goal"][1].asFloat();
+        jvo["goal"][2] = jvi["goal"][2].asFloat();
 
         if (jvi["goals"] != Json::nullValue) {
                 for (auto g: jvi["goals"]) {
@@ -176,6 +179,10 @@ int main()
                 }
         }
 
+        if (jvi["slot"] != Json::nullValue) {
+                jvo["slot"] = jvi["slot"];
+        }
+
         std::vector<CircleObstacle> co;
         std::vector<SegmentObstacle> so;
         p.HMIN = p.root()->x();
@@ -437,6 +444,9 @@ escapeloop:
                 }
                 j++;
         }
+        jvo["goals"][0][0] = p.tlog()[p.tlog().size() - 1].front()->x();
+        jvo["goals"][0][1] = p.tlog()[p.tlog().size() - 1].front()->y();
+        jvo["goals"][0][2] = p.tlog()[p.tlog().size() - 1].front()->h();
 #ifdef JSONLOGEDGES
         i = 0;
         j = 0;