diff --git a/costmap_2d/include/costmap_2d/inflation_layer.h b/costmap_2d/include/costmap_2d/inflation_layer.h index c8611224c0..79c8722d37 100644 --- a/costmap_2d/include/costmap_2d/inflation_layer.h +++ b/costmap_2d/include/costmap_2d/inflation_layer.h @@ -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_; }; diff --git a/costmap_2d/plugins/inflation_layer.cpp b/costmap_2d/plugins/inflation_layer.cpp index 5e4e0b4c23..90a88f65bc 100644 --- a/costmap_2d/plugins/inflation_layer.cpp +++ b/costmap_2d/plugins/inflation_layer.cpp @@ -223,6 +223,7 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, // We use a map 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++) @@ -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]) @@ -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]; @@ -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(); } } @@ -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); } } @@ -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() diff --git a/costmap_2d/test/inflation_tests.cpp b/costmap_2d/test/inflation_tests.cpp index 3c29c2f512..d07bf15651 100644 --- a/costmap_2d/test/inflation_tests.cpp +++ b/costmap_2d/test/inflation_tests.cpp @@ -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 @@ -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 @@ -50,16 +50,16 @@ std::vector setRadii(LayeredCostmap& layers, double length, double width, std::vector 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); @@ -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 > 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 >::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); @@ -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); } } @@ -143,7 +139,7 @@ TEST(costmap, testAdjacentToObstacleCanStillMove){ // circumscribed radius = 3.1 std::vector 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); @@ -169,7 +165,7 @@ TEST(costmap, testInflationShouldNotCreateUnknowns){ // circumscribed radius = 3.1 std::vector 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); @@ -194,7 +190,7 @@ TEST(costmap, testCostFunctionCorrectness){ // circumscribed radius = 8.0 std::vector 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); @@ -291,7 +287,7 @@ TEST(costmap, testInflation){ std::vector 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); @@ -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); @@ -355,7 +351,7 @@ TEST(costmap, testInflation2){ std::vector 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); @@ -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); } @@ -382,7 +378,7 @@ TEST(costmap, testInflation3){ // 1 2 3 std::vector 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);