Load goals if present in JSON input

diff --git a/base/main.cc b/base/main.cc
index 11fa7d425f8e50f8afce2e21d93afde8dc56acab..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();