/**
* @brief Given an index of a cell in the costmap, place it into a priority queue 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)
* @param my The y coordinate of the cell (can be computed from the index, but saves time to store it)
* @param src_x The x index of the obstacle point inflation started at
* @param src_y The y index of the obstacle point inflation started at
*/
inline void InflationLayer::enqueue(unsigned char* grid, unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y)
{
//set the cost of the cell being inserted
if (!seen_[index])
{
//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
if (distance > cell_inflation_radius_)
return;
//assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, src_x, src_y);
unsigned char old_cost = grid[index];
if (old_cost == NO_INFORMATION && cost >= INSCRIBED_INFLATED_OBSTACLE)
grid[index] = cost;
else
grid[index] = std::max(old_cost, cost);
//push the cell data onto the queue and mark
seen_[index] = true;
CellData data(distance, index, mx, my, src_x, src_y);
inflation_queue_.push(data);
}
}
/**
* @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)
* @param my The y coordinate of the cell (can be computed from the index, but saves time to store it)
* @param src_x The x index of the obstacle point inflation started at
* @param src_y The y index of the obstacle point inflation started at
*/
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y)
{
if (!seen_[index])
{
// 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 list if it is within the inflation radius of the obstacle point
if (distance > cell_inflation_radius_)
return;
// push the cell data onto the inflation list and mark
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
}
}
