Skip to content

Commit

Permalink
fix: correctly free memory in inflation layer (#107)
Browse files Browse the repository at this point in the history
* Correctly free the memory in InflationLayer

* Remove 4 redundant bytes from CellData in Inflationlayer

* Update inflation_layer.cpp

* add const and remove old comment
  • Loading branch information
Nisarg236 authored Dec 24, 2024
1 parent afa9b36 commit f31de3d
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 50 deletions.
6 changes: 2 additions & 4 deletions costmap_2d/include/costmap_2d/inflation_layer.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,18 +56,16 @@ class CellData
public:
/**
* @brief Constructor for a CellData objects
* @param i The index of the cell in the cost map
* @param x The x coordinate of the cell in the cost map
* @param y The y coordinate of the cell in the cost map
* @param sx The x coordinate of the closest obstacle cell in the costmap
* @param sy The y coordinate of the closest obstacle cell in the costmap
* @return
*/
CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
CellData(unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
x_(x), y_(y), src_x_(sx), src_y_(sy)
{
}
unsigned int index_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};
Expand Down
35 changes: 16 additions & 19 deletions costmap_2d/plugins/inflation_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -223,6 +223,7 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
// Start with lethal obstacles: by definition distance is 0.0
auto& obs_bin = inflation_cells_[0];
obs_bin.reserve(200);
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
Expand All @@ -231,19 +232,26 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
obs_bin.emplace_back(index, i, j, i, j);
obs_bin.emplace_back(i, j, i, j);
}
}
}

// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
for (const auto& dist_bin: inflation_cells_)
for (auto& dist_bin: inflation_cells_)
{
for (const auto& current_cell: dist_bin)
dist_bin.reserve(200);
for (std::size_t i = 0; i < dist_bin.size(); ++i)
{
// process all cells at distance dist_bin.first
unsigned int index = current_cell.index_;
const auto& current_cell = dist_bin[i];

unsigned int mx = current_cell.x_;
unsigned int my = current_cell.y_;
unsigned int sx = current_cell.src_x_;
unsigned int sy = current_cell.src_y_;
unsigned int index = master_grid.getIndex(mx, my);

// ignore if already visited
if (seen_[index])
Expand All @@ -253,11 +261,6 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,

seen_[index] = true;

unsigned int mx = current_cell.x_;
unsigned int my = current_cell.y_;
unsigned int sx = current_cell.src_x_;
unsigned int sy = current_cell.src_y_;

// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, sx, sy);
unsigned char old_cost = master_array[index];
Expand All @@ -276,12 +279,10 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}
}

for (auto& dist:inflation_cells_)
{
dist.clear();
dist.reserve(200);
// This level of inflation_cells_ is not needed anymore. We can free the memory
// Note that dist_bin.clear() is not enough, because it won't free the memory
dist_bin = std::vector<CellData>();
}
}

Expand All @@ -299,7 +300,7 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
const int r = cell_inflation_radius_ + 2;

// push the cell data onto the inflation list and mark
inflation_cells_[distance_matrix_[mx - src_x+r][my - src_y+r]].emplace_back(index, mx, my, src_x, src_y);
inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(mx, my, src_x, src_y);
}
}

Expand Down Expand Up @@ -339,10 +340,6 @@ void InflationLayer::computeCaches()
int max_dist = generateIntegerDistances();
inflation_cells_.clear();
inflation_cells_.resize(max_dist + 1);
for (auto& dist : inflation_cells_)
{
dist.reserve(200);
}
}

int InflationLayer::generateIntegerDistances()
Expand Down
50 changes: 23 additions & 27 deletions costmap_2d/test/inflation_tests.cpp
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
/*
* Copyright (c) 2013, Willow Garage, Inc.
* All rights reserved.
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
Expand All @@ -13,7 +13,7 @@
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
Expand Down Expand Up @@ -50,16 +50,16 @@ std::vector<Point> setRadii(LayeredCostmap& layers, double length, double width,
std::vector<Point> polygon;
Point p;
p.x = width;
p.y = length;
p.y = length;
polygon.push_back(p);
p.x = width;
p.y = -length;
p.y = -length;
polygon.push_back(p);
p.x = -width;
p.y = -length;
p.y = -length;
polygon.push_back(p);
p.x = -width;
p.y = length;
p.y = length;
polygon.push_back(p);
layers.setFootprint(polygon);

Expand All @@ -75,16 +75,16 @@ void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap
bool* seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
std::map<double, std::vector<CellData> > m;
CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
CellData initial(mx, my, mx, my);
m[0].push_back(initial);
for (std::map<double, std::vector<CellData> >::iterator bin = m.begin(); bin != m.end(); ++bin)
{
for (int i = 0; i < bin->second.size(); ++i)
{
const CellData& cell = bin->second[i];
if (!seen[cell.index_])
{
seen[cell.index_] = true;
const auto index = costmap->getIndex(cell.x_, cell.y_);
if (!seen[index]) {
seen[index] = true;
unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
double dist = hypot(dx, dy);
Expand All @@ -106,26 +106,22 @@ void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap

if (cell.x_ > 0)
{
CellData data(costmap->getIndex(cell.x_-1, cell.y_),
cell.x_-1, cell.y_, cell.src_x_, cell.src_y_);
CellData data(cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.y_ > 0)
{
CellData data(costmap->getIndex(cell.x_, cell.y_-1),
cell.x_, cell.y_-1, cell.src_x_, cell.src_y_);
CellData data(cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.x_ < costmap->getSizeInCellsX() - 1)
{
CellData data(costmap->getIndex(cell.x_+1, cell.y_),
cell.x_+1, cell.y_, cell.src_x_, cell.src_y_);
CellData data(cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.y_ < costmap->getSizeInCellsY() - 1)
{
CellData data(costmap->getIndex(cell.x_, cell.y_+1),
cell.x_, cell.y_+1, cell.src_x_, cell.src_y_);
CellData data(cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
}
Expand All @@ -143,7 +139,7 @@ TEST(costmap, testAdjacentToObstacleCanStillMove){
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);

ObstacleLayer* olayer = addObstacleLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);

Expand All @@ -169,7 +165,7 @@ TEST(costmap, testInflationShouldNotCreateUnknowns){
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);

ObstacleLayer* olayer = addObstacleLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);

Expand All @@ -194,7 +190,7 @@ TEST(costmap, testCostFunctionCorrectness){
// circumscribed radius = 8.0
std::vector<Point> polygon = setRadii(layers, 5.0, 6.25, 10.5);

ObstacleLayer* olayer = addObstacleLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);

Expand Down Expand Up @@ -291,7 +287,7 @@ TEST(costmap, testInflation){
std::vector<Point> polygon = setRadii(layers, 1, 1, 1);

addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);

Expand All @@ -317,7 +313,7 @@ TEST(costmap, testInflation){
// It and its 2 neighbors makes 3 obstacles
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51);

// @todo Rewrite
// @todo Rewrite
// Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
addObservation(olayer, 2, 0);
layers.updateMap(0,0,0);
Expand Down Expand Up @@ -355,7 +351,7 @@ TEST(costmap, testInflation2){
std::vector<Point> polygon = setRadii(layers, 1, 1, 1);

addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);

Expand All @@ -367,7 +363,7 @@ TEST(costmap, testInflation2){

Costmap2D* costmap = layers.getCostmap();
//printMap(*costmap);
ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
}

Expand All @@ -382,7 +378,7 @@ TEST(costmap, testInflation3){
// 1 2 3
std::vector<Point> polygon = setRadii(layers, 1, 1.75, 3);

ObstacleLayer* olayer = addObstacleLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);

Expand Down

0 comments on commit f31de3d

Please sign in to comment.