Remove colliding `ns` in T2::next()

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index b1e875f24cae4fe7e689598e31e773c3bb1d748e..62aaac3577510e1803c8fe7ced1c77eed44439c9 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -438,6 +438,8 @@ bool T2::next()
                         // connect
                         if (!this->connect(pn, ns, nvs)) {
                                 this->iy_[IYI(ns->y())].pop_back();
+                                this->nodes().pop_back();
+                                delete ns;
                                 en_add = false;
                         } else {
                                 // rewire
@@ -475,6 +477,8 @@ bool T2::next()
                         // connect
                         if (!this->connect(pn, ns, nvs)) {
                                 this->iy_[IYI(ns->y())].pop_back();
+                                this->nodes().pop_back();
+                                delete ns;
                                 en_add = false;
                         } else {
                                 // rewire
@@ -512,6 +516,8 @@ bool T2::next()
                                         pn->children().pop_back();
                                         ns->remove_parent();
                                         this->iy_[IYI(ns->y())].pop_back();
+                                        this->nodes().pop_back();
+                                        delete ns;
                                         en_add = false;
                                 } else {
                                         this->ocost(ns);
@@ -543,6 +549,8 @@ bool T2::next()
                                         pn->children().pop_back();
                                         ns->remove_parent();
                                         this->iy_[IYI(ns->y())].pop_back();
+                                        this->nodes().pop_back();
+                                        delete ns;
                                         en_add = false;
                                 } else {
                                         this->ocost(ns);