Output elapsed time

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

diff --git a/base/main.cc b/base/main.cc
index eb02e50ad326de9becc4e281667d4a644597b8e0..5a6d4f4193d2cba7d9104b3f1c7da2eaa31edeaa 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;
@@ -415,8 +436,20 @@ escapeloop:
                         jvo["traj"][j][i][4] = n->s();
                         i++;
                 }
+                if (j == p.tlog().size() - 1) {
+                        i = 0;
+                        for (auto n: traj) {
+                                jvo["path"][i][0] = n->x();
+                                jvo["path"][i][1] = n->y();
+                                jvo["path"][i][2] = n->h();
+                                i++;
+                        }
+                }
                 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;