Add entry as json input

[hubacji1/rrts.git] / src / rrts.cc

diff --git a/src/rrts.cc b/src/rrts.cc
index a7fd30eb086515402151dfb8b5af64c563e61315..bca083429b1ed5c5a96ec4ca7ed99c3c7ba32557 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -572,9 +572,36 @@ void RRTS::json(Json::Value jvi)
         this->nodes().front().x(jvi["init"][0].asDouble());
         this->nodes().front().y(jvi["init"][1].asDouble());
         this->nodes().front().h(jvi["init"][2].asDouble());
+        {
+            if (jvi["entry"] != Json::nullValue) {
+                this->entry_set = true;
+                this->entry.x = jvi["entry"][0].asDouble();
+                this->entry.y = jvi["entry"][1].asDouble();
+                this->entry.b = jvi["entry"][2].asDouble();
+                this->entry.e = jvi["entry"][3].asDouble();
+            } else {
+                this->entry.x = 0.0;
+                this->entry.y = 0.0;
+                this->entry.b = 0.0;
+                this->entry.e = 0.0;
+            }
+        }
         {
                 RRTNode tmp_node;
                 RRTNode* gp = nullptr;
+                if (jvi["entry"] != Json::nullValue) {
+                    this->entry_set = true;
+                    this->entry.x = jvi["entry"][0].asDouble();
+                    this->entry.y = jvi["entry"][1].asDouble();
+                    this->entry.b = jvi["entry"][2].asDouble();
+                    this->entry.e = jvi["entry"][3].asDouble();
+                    tmp_node.x(this->entry.x);
+                    tmp_node.y(this->entry.y);
+                    tmp_node.h((this->entry.b + this->entry.e) / 2.0);
+                    this->goals().push_back(tmp_node);
+                    this->goals().back().p(gp);
+                    gp = &this->goals().back();
+                }
                 for (auto g: jvi["goals"]) {
                         tmp_node.x(g[0].asDouble());
                         tmp_node.y(g[1].asDouble());