From 8db314d363c69a56a8d613bb58c3c52b3b8ce18d Mon Sep 17 00:00:00 2001
From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Fri, 29 Jun 2018 23:18:10 +0200
Subject: [PATCH] Fix memory leaks

---
 CHANGELOG.md                   |  3 +++
 base/main.cc                   | 12 ++++++++++++
 base/rrtbase.cc                | 15 ++++++++++++++-
 decision_control/rrtplanner.cc |  2 ++
 perception/obstacle.cc         | 10 ++++++++++
 5 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7458486..fc1a67f 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -22,3 +22,6 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Obstacles class (circle obstacle, segment obstacle).
 - Collision check function to base RRT class.
 - Bicycle car model used for collision check with car frame.
+
+### Fixed
+- Memory leaks.
diff --git a/base/main.cc b/base/main.cc
index f7e9d25..e82eda1 100644
--- a/base/main.cc
+++ b/base/main.cc
@@ -81,5 +81,17 @@ int main()
                 j++;
         }
         std::cout << jvo << std::endl;
+
+        // free mem
+        for (auto o: so) {
+                delete o.init();
+                delete o.goal();
+        }
+        for (auto n: p.nodes())
+                delete n;
+        for (auto s: p.samples())
+                delete s;
+        delete p.root();
+        delete p.goal();
         return 0;
 }
diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 0092db3..70dff35 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -123,6 +123,7 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
         RRTNode *tmp = goal;
         while (tmp != init) {
                 BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
+                std::vector<RRTEdge *> bcframe = bc.frame();
                 for (auto &o: *this->cobstacles_) {
                         if (o.collide(tmp)) {
                                 return true;
@@ -130,8 +131,13 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
                         // TODO collide with car frame
                 }
                 for (auto &o: *this->sobstacles_) {
-                        for (auto &e: bc.frame()) {
+                        for (auto &e: bcframe) {
                                 if (o.collide(e)) {
+                                        for (auto e: bcframe) {
+                                                delete e->init();
+                                                delete e->goal();
+                                                delete e;
+                                        }
                                         return true;
                                 }
                         }
@@ -141,6 +147,11 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
                 }
                 edges.push_back(new RRTEdge(tmp, tmp->parent()));
                 tmp = tmp->parent();
+                for (auto e: bcframe) {
+                        delete e->init();
+                        delete e->goal();
+                        delete e;
+                }
         }
         for (auto &e: edges) {
                 for (auto &o: *this->cobstacles_) {
@@ -154,6 +165,8 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
                         }
                 }
         }
+        for (auto e: edges)
+                delete e;
         return false;
 }
 
diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index f446ba4..8fc5d77 100644
--- 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();
diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 4a140e8..c16b7de 100644
--- a/perception/obstacle.cc
+++ b/perception/obstacle.cc
@@ -72,10 +72,20 @@ bool CircleObstacle::collide(std::vector<RRTEdge *> &edges)
                                                 be->init(),
                                                 be->goal())
                                         .collide(e)) {
+                                for (auto e: bedges) {
+                                        delete e->init();
+                                        delete e->goal();
+                                        delete e;
+                                }
                                 return true;
                         }
                 }
         }
+        for (auto e: bedges) {
+                delete e->init();
+                delete e->goal();
+                delete e;
+        }
         return false;
 }
 
-- 
2.39.2