From 05175d9f05440cebf8283a8de23eeca71a095ebe Mon Sep 17 00:00:00 2001
From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Thu, 4 Oct 2018 09:22:02 +0200
Subject: [PATCH] Do not remove nodes from RRT

---
 decision_control/rrtplanner.cc | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index d498380..784b30e 100644
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -632,11 +632,9 @@ bool T2::opt_part(RRTNode *init, RRTNode *goal)
         }
         RRTNode *op;
         op = init->parent();
-        if (op) {
-                op->rem_child(init);
-                op->add_child(steered[0], this->cost(op, steered[0]));
-        } else {
-                init->add_child(steered[0], this->cost(init, steered[0]));
+        if (!op)
+                op = init;
+        op->add_child(steered[0], this->cost(op, steered[0]));
         }
         steered[steered.size() - 1]->add_child(
                         goal,
-- 
2.39.2