void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}
unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
master_array[it] = costmap_[it];
it++;
}
}
}
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}
unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION)
master_array[it] = costmap_[it];
else
{
int sum = old_cost + costmap_[it];
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
else
master_array[it] = sum;
}
it++;
}
}
}
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = span*j+min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] != NO_INFORMATION)
master[it] = costmap_[it];
it++;
}
}
}
void BallPickerLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
const unsigned char* master_array = master_grid.getCharMap();
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
if (clear_flag)
{
master_grid.setCost(i, j, FREE_SPACE);
}
else
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION || old_cost < costmap_[index])
master_grid.setCost(i, j, costmap_[index]);
}
}
}
for (int i=0; i < getSizeInCellsX()*getSizeInCellsY(); i++)
costmap_[i] == NO_INFORMATION;
if (!obstacle_buffer.empty())
{
ROS_INFO("Costmap updated with ball obstacles.");
obstacle_buffer.clear();
std_srvs::Empty emptysrv;
confirm_update_client.call(emptysrv);
}
}
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
boost::unique_lock < boost::shared_mutex > lock(*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");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
min_i = std::max( 0, min_i );
min_j = std::max( 0, min_j );
max_i = std::min( int( size_x ), max_i );
max_j = std::min( int( size_y ), max_j );
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
enqueue(master_array, index, i, j, i, j);
}
}
}
while (!inflation_queue_.empty())
{
//get the highest priority cell and pop it off the priority queue
const CellData& current_cell = inflation_queue_.top();
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_;
//attempt to put the neighbors of the current cell onto the queue
if (mx > 0)
enqueue(master_array, index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(master_array, index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(master_array, index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(master_array, index + size_x, mx, my + 1, sx, sy);
//remove the current cell from the priority queue
inflation_queue_.pop();
}
}
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_ || (cell_inflation_radius_ == 0))
return;
// 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();
if (seen_ == NULL) {
ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
{
ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
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++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
obs_bin.push_back(CellData(index, 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
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{
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 = cell.index_;
// ignore if already visited
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 && (inflate_unknown_ ? (cost > FREE_SPACE) : (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);
}
}
inflation_cells_.clear();
}