Fix memory leaks

[hubacji1/iamcar.git] / decision_control / rrtplanner.cc

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index f446ba446cec46a4971af6ddad6feafb23e95a40..8fc5d77a70ab2687523f9c01536a161911633610 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -36,9 +36,11 @@ LaValle1998::LaValle1998(RRTNode *init, RRTNode *goal):
 bool LaValle1998::next()
 {
         RRTNode *rs = this->sample();
+        this->samples().push_back(rs);
         RRTNode *nn = this->nn(this->root(), rs, this->cost);
         RRTNode *pn = nn;
         for (auto ns: this->steer(nn, rs)) {
+                this->nodes().push_back(ns);
                 pn->add_child(ns, this->cost(pn, ns));
                 if (this->collide(pn, ns)) {
                         pn->children().pop_back();