Store entries

diff --git a/api/rrts.h b/api/rrts.h
index e5821a02791b890e5b211cde7a74c0e53452882b..2ce7e943ee5ec06395748f8a14610edc34a93d52 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -217,6 +217,9 @@ class RRTS {
                 /// ---
                 struct { double x=0; double y=0; double b=0; double e=0; } entry;
                 bool entry_set = false;
+                struct { double x=0; double y=0; double h=0; } entry1;
+                struct { double x=0; double y=0; double h=0; } entry2;
+                bool entries_set = false;
                 std::vector<RRTNode *> steered1_;
                 std::vector<RRTNode *> steered2_;
                 /// ---

diff --git a/src/rrts.cc b/src/rrts.cc
index 217e44ad747172fb0a2befaae3b9acd3863d0771..7a8083d46bfd4ccb065b2d699cbdae5e64a89f9f 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -617,6 +617,18 @@ Json::Value RRTS::json()
                         jvo["entry"][0] = this->goals().front().x();
                         jvo["entry"][1] = this->goals().front().y();
                         jvo["entry"][2] = this->goals().front().h();
+                        if (this->entry_set) {
+                            jvo["entry"][2] = this->entry.b;
+                            jvo["entry"][3] = this->entry.e;
+                        }
+                        if (this->entries_set) {
+                                jvo["entries"][0][0] = this->entry1.x;
+                                jvo["entries"][0][1] = this->entry1.y;
+                                jvo["entries"][0][2] = this->entry1.h;
+                                jvo["entries"][1][0] = this->entry2.x;
+                                jvo["entries"][1][1] = this->entry2.y;
+                                jvo["entries"][1][2] = this->entry2.h;
+                        }
                         jvo["goal"][0] = this->goals().back().x();
                         jvo["goal"][1] = this->goals().back().y();
                         jvo["goal"][2] = this->goals().back().h();
@@ -721,6 +733,15 @@ void RRTS::json(Json::Value jvi)
                         this->goals().back().p(gp);
                         gp = &this->goals().back();
                 }
+                if (jvi["entries"] != Json::nullValue) {
+                        this->entries_set = true;
+                        this->entry1.x = jvi["entries"][0][0].asDouble();
+                        this->entry1.y = jvi["entries"][0][1].asDouble();
+                        this->entry1.h = jvi["entries"][0][2].asDouble();
+                        this->entry2.x = jvi["entries"][1][0].asDouble();
+                        this->entry2.y = jvi["entries"][1][1].asDouble();
+                        this->entry2.h = jvi["entries"][1][2].asDouble();
+                }
                 for (auto g: jvi["goals"]) {
                         RRTNode tmp_node;
                         tmp_node.x(g[0].asDouble());