Do not remove nodes from RRT

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index d4983804d2bb69a4258579b4d56c94e46bf25cee..784b30ee606a26a8eb2ff47d0141a45512f34953 100644 (file)
--- 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,