Load goals if present in JSON input

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

diff --git a/base/main.cc b/base/main.cc
index eb02e50ad326de9becc4e281667d4a644597b8e0..2e8f7cbb46446cc81d659a8867aaf40ee876c634 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -165,6 +165,17 @@ int main()
         jvo["init"][0] = p.root()->x();
         jvo["init"][1] = p.root()->y();
         jvo["init"][2] = p.root()->h();
+
+        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()
+                        ));
+                }
+        }
+
         std::vector<CircleObstacle> co;
         std::vector<SegmentObstacle> so;
         p.HMIN = p.root()->x();
@@ -415,6 +426,15 @@ 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++;
         }
 #ifdef JSONLOGEDGES