Skip to content

Commit

Permalink
add checks for size prior to new, include cerr
Browse files Browse the repository at this point in the history
  • Loading branch information
joelziemann committed Oct 24, 2023
1 parent cf7e1b4 commit 41447e0
Show file tree
Hide file tree
Showing 2 changed files with 114 additions and 40 deletions.
89 changes: 65 additions & 24 deletions include/gmapping/grid/array2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -54,16 +54,22 @@ Array2D<Cell,debug>::Array2D(int xsize, int ysize){
}
m_xsize=xsize;
m_ysize=ysize;
if (m_xsize>0 && m_ysize>0){
if (m_xsize>0 && m_ysize>0)
{
m_cells=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; });
for (int i=0; i<m_xsize; i++)
{
m_cells[i]=CellArray(new Cell[m_ysize], [](auto ptr){ delete[] ptr; });
}
}
else{
else
{
std::cerr << __func__ << "Invalid size: " << "m_xsize= " << m_xsize << " m_ysize= " << m_ysize << " - resetting " << std::endl;
m_xsize=m_ysize=0;
m_cells=0;
m_cells.reset();
}
if (debug){
if (debug)
{
std::cerr << __func__ << std::endl;
std::cerr << "m_xsize= " << m_xsize<< std::endl;
std::cerr << "m_ysize= " << m_ysize<< std::endl;
Expand All @@ -72,23 +78,40 @@ 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){
if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize)
{
// delete memory
if (m_cells)
{
m_cells.reset();
}
m_xsize=g.m_xsize;
m_ysize=g.m_ysize;
m_cells=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; });
for (int i=0; i<m_xsize; i++)
m_cells[i]=CellArray(new Cell[m_ysize], [](auto ptr){ delete[] ptr; });
if (m_xsize > 0 && m_ysize > 0)
{
m_cells = CellArray2d(new CellArray[m_xsize], [](auto ptr) { delete[] ptr; });
for (int i = 0; i < m_xsize; i++)
{
m_cells[i] = CellArray(new Cell[m_ysize], [](auto ptr) { delete[] ptr; });
}
}
else
{
std::cerr << __func__ << "Invalid size: " << "m_xsize= " << m_xsize << " m_ysize= " << m_ysize << " - resetting " << std::endl;
m_xsize = m_ysize = 0;
m_cells.reset();
}
}
for (int x=0; x<m_xsize; x++)
for (int y=0; y<m_ysize; y++)
m_cells[x][y]=g.m_cells[x][y];
{
for (int y = 0; y < m_ysize; y++)
{
m_cells[x][y] = g.m_cells[x][y];
}
}

if (debug){
if (debug)
{
std::cerr << __func__ << std::endl;
std::cerr << "m_xsize= " << m_xsize<< std::endl;
std::cerr << "m_ysize= " << m_ysize<< std::endl;
Expand All @@ -97,20 +120,33 @@ 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){
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=CellArray2d(new CellArray[m_xsize], [](auto ptr){ delete[] ptr; });
for (int x=0; x<m_xsize; x++){
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];
}
if (m_xsize > 0 && m_ysize > 0)
{
m_cells = CellArray2d(new CellArray[m_xsize], [](auto ptr) { delete[] ptr; });
for (int x = 0; x < m_xsize; x++)
{
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];
}
}
}
else
{
std::cerr << __func__ << "Invalid size: " << "m_xsize= " << m_xsize << " m_ysize= " << m_ysize << " - resetting " << std::endl;
m_xsize=m_ysize=0;
m_cells.reset();
}
if (debug){
std::cerr << __func__ << std::endl;
std::cerr << "m_xsize= " << m_xsize<< std::endl;
Expand All @@ -122,8 +158,8 @@ template <class Cell, const bool debug>
Array2D<Cell,debug>::~Array2D(){
if (debug){
std::cerr << __func__ << std::endl;
std::cerr << "m_xsize= " << m_xsize<< std::endl;
std::cerr << "m_ysize= " << m_ysize<< std::endl;
std::cerr << "m_xsize= " << m_xsize<< std::endl;
std::cerr << "m_ysize= " << m_ysize<< std::endl;
}
// delete memory
if (m_cells)
Expand All @@ -136,8 +172,8 @@ template <class Cell, const bool debug>
void Array2D<Cell,debug>::clear(){
if (debug){
std::cerr << __func__ << std::endl;
std::cerr << "m_xsize= " << m_xsize<< std::endl;
std::cerr << "m_ysize= " << m_ysize<< std::endl;
std::cerr << "m_xsize= " << m_xsize<< std::endl;
std::cerr << "m_ysize= " << m_ysize<< std::endl;
}
// delete memory
if (m_cells)
Expand All @@ -151,8 +187,13 @@ void Array2D<Cell,debug>::clear(){

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;
int xsize = xmax-xmin;
int ysize = ymax-ymin;
if (xsize <= 0 || ysize <= 0)
{
std::cerr << __func__ << "Invalid reset size: " << "xsize= " << xsize << " ysize= " << ysize << " - no-op " << std::endl;
return;
}
CellArray2d newcells=CellArray2d(new CellArray[xsize], [](auto ptr){ delete[] ptr; });
for (int x=0; x<xsize; x++){
newcells[x]=CellArray(new Cell[ysize], [](auto ptr){ delete[] ptr; });;
Expand Down
65 changes: 49 additions & 16 deletions include/gmapping/grid/harray2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -64,34 +64,55 @@ HierarchicalArray2D<Cell>::HierarchicalArray2D(const HierarchicalArray2D& hg)
{
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]=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];
}
this->m_patchMagnitude=hg.m_patchMagnitude;
this->m_patchSize=hg.m_patchSize;
if (this->m_xsize > 0 && this->m_ysize > 0)
{
// 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] = 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];
}
}
this->m_patchMagnitude = hg.m_patchMagnitude;
this->m_patchSize = hg.m_patchSize;
}
else
{
std::cerr << __func__ << "Invalid size: " << "m_xsize= " << this->m_xsize << " m_ysize= " << this->m_ysize << " - resetting " << std::endl;
this->m_xsize = this->m_ysize=0;
this->m_cells.reset();
}
}

template <class Cell>
void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
int xsize=xmax-xmin;
int ysize=ymax-ymin;
if (xsize <= 0 || ysize <= 0)
{
std::cerr << __func__ << "Invalid reset size: " << "xsize= " << xsize << " ysize= " << ysize << " - no-op " << std::endl;
return;
}
HCellArray2d newcells=HCellArray2d(new HCellArray[xsize], [](auto ptr){ delete[] ptr; });
for (int x=0; x<xsize; x++){
for (int x=0; x<xsize; x++)
{
newcells[x]=HCellArray(new HCell[ysize], [](auto ptr){ delete[] ptr; });
for (int y=0; y<ysize; y++){
for (int y=0; y<ysize; y++)
{
newcells[x][y]=HCell();
}
}
int dx= xmin < 0 ? 0 : xmin;
int dy= ymin < 0 ? 0 : ymin;
int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
for (int x=dx; x<Dx; x++){
for (int y=dy; y<Dy; y++){
for (int x=dx; x<Dx; x++)
{
for (int y=dy; y<Dy; y++)
{
newcells[x-xmin][y-ymin]=this->m_cells[x][y];
}
}
Expand All @@ -107,9 +128,21 @@ HierarchicalArray2D<Cell>& HierarchicalArray2D<Cell>::operator=(const Hierarchic
this->m_cells.reset();
this->m_xsize=hg.m_xsize;
this->m_ysize=hg.m_ysize;
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]=HCellArray(new HCell[this->m_ysize]);

if (this->m_xsize > 0 && this->m_ysize > 0)
{
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] = HCellArray(new HCell[this->m_ysize]);
}
}
else
{
std::cerr << __func__ << "Invalid size: " << "m_xsize= " << this->m_xsize << " m_ysize= " << this->m_ysize << " - resetting " << std::endl;
this->m_xsize = this->m_ysize = 0;
this->m_cells.reset();
}
}
for (int x=0; x<this->m_xsize; x++)
for (int y=0; y<this->m_ysize; y++)
Expand Down

0 comments on commit 41447e0

Please sign in to comment.