From 7396e67dbf127b12d47b6adc71209de66cf83f27 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Thu, 12 Nov 2020 11:16:31 +0100
Subject: [PATCH] When loading goals, set parent and type

---
 src/rrts.cc | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/rrts.cc b/src/rrts.cc
index a727019..63da239 100644
--- 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;
-- 
2.39.2