From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Thu, 4 Oct 2018 07:22:02 +0000 (+0200)
Subject: Do not remove nodes from RRT
X-Git-Tag: v0.2.0~9^2~2
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/05175d9f05440cebf8283a8de23eeca71a095ebe

Do not remove nodes from RRT
---

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,