Issue 42 - changes for slow memory leak

CMakeLists.txt

@@ -1,9 +1,11 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(openslam_gmapping)
 
 ## Find catkin macros and libraries
 find_package(catkin REQUIRED)
 
+add_compile_options(-std=c++17)
+
 include(GenerateExportHeader)
 set(EXPORT_HEADER_DIR "${CATKIN_DEVEL_PREFIX}/include")
 file(MAKE_DIRECTORY "${EXPORT_HEADER_DIR}")

gridfastslam/gridslamprocessor.cpp

@@ -409,8 +409,8 @@ void GridSlamProcessor::setMotionModelParameters
       }
       m_infoStream << "m_count " << m_count << endl;
 
-      RangeReading* reading_copy = 
-              new RangeReading(reading.size(),
+      std::shared_ptr<RangeReading> reading_copy =
+              std::make_shared<RangeReading>(reading.size(),
                                &(reading[0]),
                                static_cast<const RangeSensor*>(reading.getSensor()),
                                reading.getTime());
@@ -466,7 +466,8 @@ void GridSlamProcessor::setMotionModelParameters
       updateTreeWeights(false);
       //		cerr  << ".done!" <<endl;
 
-      delete [] plainReading;
+      delete[] plainReading;
+      plainReading=0;
       m_lastPartPose=m_odoPose; //update the past pose for the next iteration
       m_linearDistance=0;
       m_angularDistance=0;

include/gmapping/grid/array2d.h

@@ -1,6 +1,7 @@
 #ifndef ARRAY2D_H
 #define ARRAY2D_H
 
+#include <memory>
 #include <assert.h>
 #include <gmapping/utils/point.h>
 #include "gmapping/grid/accessstate.h"
@@ -11,6 +12,8 @@ namespace GMapping {
 
 template<class Cell, const bool debug=false> class Array2D{
 	public:
+    using CellArray = std::shared_ptr<Cell[]>;
+    using CellArray2d = std::shared_ptr<CellArray[]>;
 		Array2D(int xsize=0, int ysize=0);
 		Array2D& operator=(const Array2D &);
 		Array2D(const Array2D<Cell,debug> &);
@@ -33,23 +36,28 @@ template<class Cell, const bool debug=false> class Array2D{
 		inline int getPatchMagnitude() const{return 0;}
 		inline int getXSize() const {return m_xsize;}
 		inline int getYSize() const {return m_ysize;}
-		inline Cell** cells() {return m_cells;}
-		Cell ** m_cells;
+		inline CellArray2d cells() {return m_cells;}
+    CellArray2d m_cells;
 	protected:
-		int m_xsize, m_ysize;
+		int m_xsize = 0, m_ysize = 0;
 };
 
 
 template <class Cell, const bool debug>
 Array2D<Cell,debug>::Array2D(int xsize, int ysize){
 //	assert(xsize>0);
 //	assert(ysize>0);
-	m_xsize=xsize;
-	m_ysize=ysize;
+  // delete memory
+  if (m_cells)
+  {
+    m_cells.reset();
+  }
+  m_xsize=xsize;
+  m_ysize=ysize;
 	if (m_xsize>0 && m_ysize>0){
-		m_cells=new Cell*[m_xsize];
+    m_cells=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; });
 		for (int i=0; i<m_xsize; i++)
-			m_cells[i]=new Cell[m_ysize];
+      m_cells[i]=CellArray(new Cell[m_ysize], [](auto ptr){ delete[] ptr; });
 	}
 	else{
 		m_xsize=m_ysize=0;
@@ -65,14 +73,16 @@ Array2D<Cell,debug>::Array2D(int xsize, int ysize){
 template <class Cell, const bool debug>
 Array2D<Cell,debug> & Array2D<Cell,debug>::operator=(const Array2D<Cell,debug> & g){
 	if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize){
-		for (int i=0; i<m_xsize; i++)
-			delete [] m_cells[i];
-		delete [] m_cells;
+    // delete memory
+    if (m_cells)
+    {
+      m_cells.reset();
+    }
 		m_xsize=g.m_xsize;
 		m_ysize=g.m_ysize;
-		m_cells=new Cell*[m_xsize];
+    m_cells=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; });
 		for (int i=0; i<m_xsize; i++)
-			m_cells[i]=new Cell[m_ysize];
+      m_cells[i]=CellArray(new Cell[m_ysize], [](auto ptr){ delete[] ptr; });
 	}
 	for (int x=0; x<m_xsize; x++)
 		for (int y=0; y<m_ysize; y++)
@@ -88,11 +98,16 @@ Array2D<Cell,debug> & Array2D<Cell,debug>::operator=(const Array2D<Cell,debug> &
 
 template <class Cell, const bool debug>
 Array2D<Cell,debug>::Array2D(const Array2D<Cell,debug> & g){
+  // delete memory
+  if (m_cells)
+  {
+    m_cells.reset();
+  }
 	m_xsize=g.m_xsize;
 	m_ysize=g.m_ysize;
-	m_cells=new Cell*[m_xsize];
+  m_cells=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; });
 	for (int x=0; x<m_xsize; x++){
-		m_cells[x]=new Cell[m_ysize];
+    m_cells[x]=CellArray(new Cell[m_ysize], [](auto ptr){ delete[] ptr; });
 		for (int y=0; y<m_ysize; y++)
 			m_cells[x][y]=g.m_cells[x][y];
 	}
@@ -110,12 +125,11 @@ Array2D<Cell,debug>::~Array2D(){
 	std::cerr << "m_xsize= " << m_xsize<< std::endl;
 	std::cerr << "m_ysize= " << m_ysize<< std::endl;
   }
-  for (int i=0; i<m_xsize; i++){
-    delete [] m_cells[i];
-    m_cells[i]=0;
+  // delete memory
+  if (m_cells)
+  {
+    m_cells.reset();
   }
-  delete [] m_cells;
-  m_cells=0;
 }
 
 template <class Cell, const bool debug>
@@ -125,12 +139,11 @@ void Array2D<Cell,debug>::clear(){
 	std::cerr << "m_xsize= " << m_xsize<< std::endl;
 	std::cerr << "m_ysize= " << m_ysize<< std::endl;
   }
-  for (int i=0; i<m_xsize; i++){
-    delete [] m_cells[i];
-    m_cells[i]=0;
+  // delete memory
+  if (m_cells)
+  {
+    m_cells.reset();
   }
-  delete [] m_cells;
-  m_cells=0;
   m_xsize=0;
   m_ysize=0;
 }
@@ -140,9 +153,9 @@ template <class Cell, const bool debug>
 void Array2D<Cell,debug>::resize(int xmin, int ymin, int xmax, int ymax){
 	int xsize=xmax-xmin;
 	int ysize=ymax-ymin;
-	Cell ** newcells=new Cell *[xsize];
+  CellArray2d newcells=CellArray2d(new CellArray[xsize], [](auto ptr){ delete[] ptr; });
 	for (int x=0; x<xsize; x++){
-		newcells[x]=new Cell[ysize];
+    newcells[x]=CellArray(new Cell[ysize], [](auto ptr){ delete[] ptr; });;
 	}
 	int dx= xmin < 0 ? 0 : xmin;
 	int dy= ymin < 0 ? 0 : ymin;
@@ -152,9 +165,7 @@ void Array2D<Cell,debug>::resize(int xmin, int ymin, int xmax, int ymax){
 		for (int y=dy; y<Dy; y++){
 			newcells[x-xmin][y-ymin]=this->m_cells[x][y];
 		}
-		delete [] this->m_cells[x];
 	}
-	delete [] this->m_cells;
 	this->m_cells=newcells;
 	this->m_xsize=xsize;
 	this->m_ysize=ysize; 

include/gmapping/grid/harray2d.h

@@ -1,15 +1,19 @@
 #ifndef HARRAY2D_H
 #define HARRAY2D_H
 #include <set>
+#include <memory>
 #include <gmapping/utils/point.h>
 #include <gmapping/utils/autoptr.h>
 #include "gmapping/grid/array2d.h"
 
 namespace GMapping {
 
 template <class Cell>
-class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
+class HierarchicalArray2D: public Array2D< std::shared_ptr< Array2D<Cell> > >{
 	public:
+    using HCell = std::shared_ptr< Array2D<Cell> >;
+    using HCellArray = std::shared_ptr<HCell[]>;
+    using HCellArray2d = std::shared_ptr<HCellArray[]>;
 		typedef std::set< point<int>, pointcomparator<int> > PointSet;
 		HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5);
 		HierarchicalArray2D(const HierarchicalArray2D& hg);
@@ -43,20 +47,27 @@ class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
 
 template <class Cell>
 HierarchicalArray2D<Cell>::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude) 
-  :Array2D<autoptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
+  :Array2D<std::shared_ptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
 	m_patchMagnitude=patchMagnitude;
 	m_patchSize=1<<m_patchMagnitude;
+    // this->m_xsize = (xsize>>patchMagnitude);
+    // this->m_ysize = (ysize>>patchMagnitude);
 }
 
 template <class Cell>
 HierarchicalArray2D<Cell>::HierarchicalArray2D(const HierarchicalArray2D& hg)
-  :Array2D<autoptr< Array2D<Cell> > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude))  // added by cyrill: if you have a resize error, check this again
+  :Array2D<std::shared_ptr< Array2D<Cell> > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude))  // added by cyrill: if you have a resize error, check this again
 {
 	this->m_xsize=hg.m_xsize;
 	this->m_ysize=hg.m_ysize;
-	this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
+  if (this->m_cells)
+  {
+    this->m_cells.reset();
+  }
+	// the Array2D m_cells is a std::shared_ptr<std::shared_ptr<Cell[]>[]>, where Cell is std::shared_ptr< Array2D<Cell> >
+  this->m_cells=HCellArray2d(new HCellArray[this->m_xsize], [](auto ptr){ delete[] ptr; });
 	for (int x=0; x<this->m_xsize; x++){
-		this->m_cells[x]=new autoptr< Array2D<Cell> >[this->m_ysize];
+    this->m_cells[x]=HCellArray(new HCell[this->m_ysize]);
 		for (int y=0; y<this->m_ysize; y++)
 			this->m_cells[x][y]=hg.m_cells[x][y];
 	}
@@ -68,11 +79,11 @@ template <class Cell>
 void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
 	int xsize=xmax-xmin;
 	int ysize=ymax-ymin;
-	autoptr< Array2D<Cell> > ** newcells=new autoptr< Array2D<Cell> > *[xsize];
+  HCellArray2d newcells=HCellArray2d(new HCellArray[xsize], [](auto ptr){ delete[] ptr; });
 	for (int x=0; x<xsize; x++){
-		newcells[x]=new autoptr< Array2D<Cell> >[ysize];
+    newcells[x]=HCellArray(new HCell[ysize], [](auto ptr){ delete[] ptr; });
 		for (int y=0; y<ysize; y++){
-			newcells[x][y]=autoptr< Array2D<Cell> >(0);
+      newcells[x][y]=HCell();
 		}
 	}
 	int dx= xmin < 0 ? 0 : xmin;
@@ -83,9 +94,7 @@ void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
 		for (int y=dy; y<Dy; y++){
 			newcells[x-xmin][y-ymin]=this->m_cells[x][y];
 		}
-		delete [] this->m_cells[x];
 	}
-	delete [] this->m_cells;
 	this->m_cells=newcells;
 	this->m_xsize=xsize;
 	this->m_ysize=ysize; 
@@ -95,14 +104,12 @@ template <class Cell>
 HierarchicalArray2D<Cell>& HierarchicalArray2D<Cell>::operator=(const HierarchicalArray2D& hg){
 //	Array2D<autoptr< Array2D<Cell> > >::operator=(hg);
 	if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){
-		for (int i=0; i<this->m_xsize; i++)
-			delete [] this->m_cells[i];
-		delete [] this->m_cells;
+		this->m_cells.reset();
 		this->m_xsize=hg.m_xsize;
 		this->m_ysize=hg.m_ysize;
-		this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
+    this->m_cells=HCellArray2d(new HCellArray[this->m_xsize], [](auto ptr){ delete[] ptr; });
 		for (int i=0; i<this->m_xsize; i++)
-			this->m_cells[i]=new autoptr< Array2D<Cell> > [this->m_ysize];
+      this->m_cells[i]=HCellArray(new HCell[this->m_ysize]);
 	}
 	for (int x=0; x<this->m_xsize; x++)
 		for (int y=0; y<this->m_ysize; y++)
@@ -137,32 +144,34 @@ Array2D<Cell>* HierarchicalArray2D<Cell>::createPatch(const IntPoint& ) const{
 template <class Cell>
 AccessibilityState  HierarchicalArray2D<Cell>::cellState(int x, int y) const {
 	if (this->isInside(patchIndexes(x,y)))
+  {
 		if(isAllocated(x,y))
 			return (AccessibilityState)((int)Inside|(int)Allocated);
 		else
 			return Inside;
+  }
 	return Outside;
 }
 
 template <class Cell>
 void HierarchicalArray2D<Cell>::allocActiveArea(){
 	for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); it++){
-		const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y];
+		const HCell ptr=this->m_cells[it->x][it->y];
 		Array2D<Cell>* patch=0;
 		if (!ptr){
 			patch=createPatch(*it);
 		} else{	
 			patch=new Array2D<Cell>(*ptr);
 		}
-		this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);
+		this->m_cells[it->x][it->y]=HCell(patch);
 	}
 }
 
 template <class Cell>
 bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const{
 	IntPoint c=patchIndexes(x,y);
-	autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
-	return (ptr != 0);
+  HCell ptr=this->m_cells[c.x][c.y];
+	return (ptr.get() != nullptr);
 }
 
 template <class Cell>
@@ -178,18 +187,18 @@ Cell& HierarchicalArray2D<Cell>::cell(int x, int y){
 	assert(this->isInside(c.x, c.y));
 	if (!this->m_cells[c.x][c.y]){
 		Array2D<Cell>* patch=createPatch(IntPoint(x,y));
-		this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
+    this->m_cells[c.x][c.y]=HCell(patch);
 		//cerr << "!!! FATAL: your dick is going to fall down" << endl;
 	}
-	autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
+	HCell ptr=this->m_cells[c.x][c.y];
 	return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
 }
 
 template <class Cell>
 const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const{
 	assert(isAllocated(x,y));
 	IntPoint c=patchIndexes(x,y);
-	const autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
+	const HCell ptr=this->m_cells[c.x][c.y];
 	return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
 }
 

include/gmapping/gridfastslam/gridslamprocessor.h

@@ -5,6 +5,7 @@
 #include <limits>
 #include <fstream>
 #include <vector>
+#include <memory>
 #include <deque>
 #include <gmapping/particlefilter/particlefilter.h>
 #include <gmapping/utils/point.h>
@@ -70,7 +71,7 @@ namespace GMapping {
       TNode* parent;
 
       /**The range reading to which this node is associated*/
-      const RangeReading* reading;
+      std::shared_ptr<RangeReading> reading;
 
       /**The number of childs*/
       unsigned int childs;
@@ -318,7 +319,7 @@ namespace GMapping {
 
     // return if a resampling occured or not
     inline bool resample(const double* plainReading, int adaptParticles, 
-			 const RangeReading* rr=0);
+        std::shared_ptr<RangeReading> rr=std::shared_ptr<RangeReading>());
 
     //tree utilities
 

include/gmapping/gridfastslam/gridslamprocessor.hxx

@@ -67,7 +67,7 @@ inline void GridSlamProcessor::normalize(){
 
 }
 
-inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){
+inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, std::shared_ptr<RangeReading> reading){
 
   bool hasResampled = false;
 

include/gmapping/particlefilter/particlefilter.h

@@ -139,8 +139,10 @@ void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){
 		}
 	}
 	if (count>0)
+  {
 		*out=std::make_pair(current,count);
 		out++;
+  }
 }
 
 //BEGIN legacy