When loading goals, set parent and type

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

diff --git a/src/rrts.cc b/src/rrts.cc
index a7270197c1df76221e81e36cb573a6aa871bf5a4..63da239f2acedfc2992d881e16d27641c3121657 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -561,12 +561,17 @@ void RRTS::json(Json::Value jvi)
         this->nodes().front().h(jvi["init"][2].asDouble());
         {
                 RRTNode tmp_node;
+                RRTNode* gp = nullptr;
                 for (auto g: jvi["goals"]) {
                         tmp_node.x(g[0].asDouble());
                         tmp_node.y(g[1].asDouble());
                         tmp_node.h(g[2].asDouble());
                         this->goals().push_back(tmp_node);
+                        this->goals().back().p(gp);
+                        gp = &this->goals().back();
                 }
+                this->goals().front().set_t(RRTNodeType::cusp);
+                this->goals().back().set_t(RRTNodeType::cusp);
         }
         {
                 Obstacle tmp_obstacle;