Merge branch 'feature/new-json-scenario-format'

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

diff --git a/base/main.cc b/base/main.cc
index 11fa7d425f8e50f8afce2e21d93afde8dc56acab..aad156185eb4f3c9e10f7bf62918aeb3c92fbbbe 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -162,9 +162,28 @@ int main()
                         jvi["goal"][2].asFloat()
                 )
         );
+        p.firsts().push(p.goal());
         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"]) {
+                        p.goals().push_back(new RRTNode(
+                                g[0].asFloat(),
+                                g[1].asFloat(),
+                                g[2].asFloat()
+                        ));
+                }
+        }
+
+        if (jvi["slot"] != Json::nullValue) {
+                jvo["slot"] = jvi["slot"];
+        }
+
         std::vector<CircleObstacle> co;
         std::vector<SegmentObstacle> so;
         p.HMIN = p.root()->x();
@@ -258,6 +277,8 @@ int main()
                         p.tlog(p.findt());
                 }
         }
+        p.tend();
+        jvo["time"] = p.elapsed();
 #elif defined USE_PTHREAD
         bool gf = false;
         RRTNode *ron = nullptr;
@@ -394,8 +415,9 @@ escapeloop:
         jvo["nodo"][0] = nodo;
 #endif
         // log cost
-        for (j = 0; j < p.clog().size(); j++)
-                jvo["cost"][j] = p.clog()[j];
+        //for (j = 0; j < p.clog().size(); j++)
+        //        jvo["cost"][j] = p.clog()[j];
+        jvo["cost"] = p.tlog()[p.tlog().size() - 1].front()->ccost();
         // log #nodes
         for (j = 0; j < p.nlog().size(); j++)
                 jvo["node"][j] = p.nlog()[j];
@@ -426,6 +448,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;