Skip to content

Commit

Permalink
Speedup (~60%) inflation layer update (#525)
Browse files Browse the repository at this point in the history 
* Replace the priority_queue used on inflation layer by a map containing cells to inflate split by distance from the closest obstacle

* Update inflation test to match previous change
  • Loading branch information
@corot @mikeferguson
corot authored and mikeferguson committed Aug 1, 2017
1 parent d443c44 commit b2d5079
Show file tree
Hide file tree
Showing 3 changed files with 105 additions and 108 deletions.
18 changes: 3 additions & 15 deletions costmap_2d/include/costmap_2d/inflation_layer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,6 @@
#include <costmap_2d/InflationPluginConfig.h>
#include <dynamic_reconfigure/server.h>
#include <boost/thread.hpp>
#include <queue>

namespace costmap_2d
{
Expand All @@ -57,33 +56,22 @@ class CellData
public:
/**
* @brief Constructor for a CellData objects
* @param d The distance to the nearest obstacle, used for ordering in the priority queue
* @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 d, double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
distance_(d), index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
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)
{
}
double distance_;
unsigned int index_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};

/**
* @brief Provide an ordering between CellData objects in the priority queue
* @return We want the lowest distance to have the highest priority... so this returns true if a has higher priority than b
*/
inline bool operator<(const CellData &a, const CellData &b)
{
return a.distance_ > b.distance_;
}

class InflationLayer : public Layer
{
public:
Expand Down Expand Up @@ -185,7 +173,7 @@ class InflationLayer : public Layer
double inflation_radius_, inscribed_radius_, weight_;
unsigned int cell_inflation_radius_;
unsigned int cached_cell_inflation_radius_;
std::priority_queue<CellData> inflation_queue_;
std::map<double, std::vector<CellData> > inflation_cells_;

double resolution_;

Expand Down
97 changes: 53 additions & 44 deletions costmap_2d/plugins/inflation_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,15 +169,14 @@ void InflationLayer::onFootprintChanged()
layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
}

void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (!enabled_)
return;

// make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");

unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
Expand Down Expand Up @@ -210,6 +209,11 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
max_i = std::min(int(size_x), max_i);
max_j = std::min(int(size_y), max_j);

// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
// 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
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
Expand All @@ -218,55 +222,61 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
enqueue(index, i, j, i, j);
obs_bin.push_back(CellData(index, i, j, i, j));
}
}
}

while (!inflation_queue_.empty())
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{
// get the highest priority cell and pop it off the priority queue
const CellData& current_cell = inflation_queue_.top();
for (int i = 0; i < bin->second.size(); ++i)
{
// process all cells at distance dist_bin.first
const CellData& cell = bin->second[i];

unsigned int index = current_cell.index_;
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 = cell.index_;

// pop once we have our cell info
inflation_queue_.pop();
// ignore if already visited
if (seen_[index])
{
continue;
}

// set the cost of the cell being inserted
if (seen_[index])
{
continue;
seen_[index] = true;

unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = 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];
if (old_cost == NO_INFORMATION && cost >= INSCRIBED_INFLATED_OBSTACLE)
master_array[index] = cost;
else
master_array[index] = std::max(old_cost, cost);

// attempt to put the neighbors of the current cell onto the inflation list
if (mx > 0)
enqueue(index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}

seen_[index] = true;

// 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];
if (old_cost == NO_INFORMATION && cost >= INSCRIBED_INFLATED_OBSTACLE)
master_array[index] = cost;
else
master_array[index] = std::max(old_cost, cost);

// attempt to put the neighbors of the current cell onto the queue
if (mx > 0)
enqueue(index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}

inflation_cells_.clear();
}

/**
* @brief Given an index of a cell in the costmap, place it into a priority queue for obstacle inflation
* @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
* @param grid The costmap
* @param index The index of the cell
* @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
Expand All @@ -282,13 +292,12 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
// we compute our distance table one cell further than the inflation radius dictates so we can make the check below
double distance = distanceLookup(mx, my, src_x, src_y);

// we only want to put the cell in the queue if it is within the inflation radius of the obstacle point
// we only want to put the cell in the list if it is within the inflation radius of the obstacle point
if (distance > cell_inflation_radius_)
return;

// push the cell data onto the queue and mark
CellData data(distance, index, mx, my, src_x, src_y);
inflation_queue_.push(data);
// push the cell data onto the inflation list and mark
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
}
}

Expand Down
98 changes: 49 additions & 49 deletions costmap_2d/test/inflation_tests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,15 +31,14 @@
* @author David Lu!!
* Test harness for InflationLayer for Costmap2D
*/
#include <map>

#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/testing_helper.h>
#include <queue>
#include <set>
#include <gtest/gtest.h>
#include <tf/transform_listener.h>

Expand Down Expand Up @@ -75,54 +74,55 @@ 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::priority_queue<CellData> q;
CellData initial(0, costmap->getIndex(mx, my), mx, my, mx, my);
q.push(initial);
while (!q.empty())
std::map<double, std::vector<CellData> > m;
CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
m[0].push_back(initial);
for (std::map<double, std::vector<CellData> >::iterator bin = m.begin(); bin != m.end(); ++bin)
{
const CellData& cell = q.top();
if (!seen[cell.index_])
for (int i = 0; i < bin->second.size(); ++i)
{
seen[cell.index_] = true;
unsigned int dx = abs(cell.x_ - cell.src_x_);
unsigned int dy = abs(cell.y_ - cell.src_y_);
double dist = hypot(dx, dy);

unsigned char expected_cost = ilayer->computeCost(dist);
ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);

if (dist > inflation_radius)
{
q.pop();
continue;
}

if (cell.x_ > 0)
{
CellData data(dist, costmap->getIndex(cell.x_-1, cell.y_),
cell.x_-1, cell.y_, cell.src_x_, cell.src_y_);
q.push(data);
}
if (cell.y_ > 0)
{
CellData data(dist, costmap->getIndex(cell.x_, cell.y_-1),
cell.x_, cell.y_-1, cell.src_x_, cell.src_y_);
q.push(data);
}
if (cell.x_ < costmap->getSizeInCellsX() - 1)
{
CellData data(dist, costmap->getIndex(cell.x_+1, cell.y_),
cell.x_+1, cell.y_, cell.src_x_, cell.src_y_);
q.push(data);
}
if (cell.y_ < costmap->getSizeInCellsY() - 1)
const CellData& cell = bin->second[i];
if (!seen[cell.index_])
{
CellData data(dist, costmap->getIndex(cell.x_, cell.y_+1),
cell.x_, cell.y_+1, cell.src_x_, cell.src_y_);
q.push(data);
seen[cell.index_] = true;
unsigned int dx = abs(cell.x_ - cell.src_x_);
unsigned int dy = abs(cell.y_ - cell.src_y_);
double dist = hypot(dx, dy);

unsigned char expected_cost = ilayer->computeCost(dist);
ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);

if (dist > inflation_radius)
{
continue;
}

if (cell.x_ > 0)
{
CellData data(costmap->getIndex(cell.x_-1, cell.y_),
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_);
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_);
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_);
m[dist].push_back(data);
}
}
}
q.pop();
}
delete[] seen;
}
Expand Down Expand Up @@ -242,11 +242,11 @@ TEST(costmap, testCostFunctionCorrectness){

/**
* Test that there is no regression and that costs do not get
* underestimated due to the underlying priority queue being
* misused. This is a more thorough test of the cost function being
* correctly applied.
* underestimated with the distance-as-key map used to replace
* the previously used priority queue. This is a more thorough
* test of the cost function being correctly applied.
*/
TEST(costmap, testPriorityQueueUseCorrectness){
TEST(costmap, testInflationOrderCorrectness){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
Expand Down

0 comments on commit b2d5079

Please sign in to comment.