void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!map_received_)
return;
if (!layered_costmap_->isRolling())
{
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
if (!use_maximum_)
updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
else
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
}
else
{
// If rolling window, the master_grid is unlikely to have same coordinates as this layer
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
tf::StampedTransform transform;
try
{
tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
return;
}
// Copy map data given proper transformations
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
{
// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf::Point p(wx, wy, 0);
p = transform(p);
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
if (!use_maximum_)
master_grid.setCost(i, j, getCost(mx, my));
else
master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
}
}
}
}
}
void upTo(costmap_2d::Costmap2D &gradient_){
for (int varX = 0; varX < gradient_.getSizeInCellsX(); varX++) {
for (int varY = 0; varY < gradient_.getSizeInCellsY(); varY++) {
//if(gradient_.getCost(varX, varY) == 0)
gradient_.setCost(varX, varY, DEF);
}
}
}
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 SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
unsigned int mx;
unsigned int my;
if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
master_grid.setCost(mx, my, LETHAL_OBSTACLE);
}
}
void VirtualWallLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
master_grid.setCost(i, j, costmap_[index]);
}
}
}
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
unsigned int mx;
unsigned int my;
if(pode_marcar){
if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
for(int i = -1; i < 1; i++)
master_grid.setCost(mx+5, my+i, LETHAL_OBSTACLE);
}
pode_marcar = false;
}
// ROS_INFO("Hi!");
}
void spread(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int value, int i, int j, int startCellI, int startCellJ)
{
gradient_.setCost(i, j, value);
//ROS_INFO("(%d, %d) <- %d", i, j, value);
value = value + 5;
if(value>254)
value=254;
if(go(gradient_, original_, i-1,j, value))
spread(gradient_, original_, value, i-1, j, startCellI, startCellJ);
if(go(gradient_, original_, i,j-1, value))
spread(gradient_, original_, value, i, j-1, startCellI, startCellJ);
if(go(gradient_, original_, i+1,j, value))
spread(gradient_, original_, value, i+1, j, startCellI, startCellJ);
if(go(gradient_, original_, i,j+1, value))
spread(gradient_, original_, value, i, j+1, startCellI, startCellJ);
}
// method for applyting changes in this layer to global costmap
void HANPLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
int max_i, int max_j)
{
boost::recursive_mutex::scoped_lock lock(configuration_mutex_);
if(!enabled_ || (lastTransformedHumans.size() == 0))
{
return;
}
// update map according to each human position
for(auto human : lastTransformedHumans)
{
// get position of human in map
unsigned int cell_x, cell_y;
if(!master_grid.worldToMap(human.pose.position.x, human.pose.position.y, cell_x, cell_y))
{
ROS_ERROR("hanp_layer: no world coordinates for human at x:%f y:%f",
human.pose.position.x, human.pose.position.y);
continue;
}
// general algorithm is to
// create satic grids according to human posture, standing, sitting, walking
// to calcuate cost use a sigmoid function
// however 'sitting' posture is not implemented yet
if(use_safety)
{
if(safety_grid == NULL)
{
ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as safety_grid is empty");
return;
}
// apply safety grid according to position of each human
auto size_x = (int)(safety_max / resolution), size_y = size_x;
// by convention x is number of columns, y is number of rows
for(int y = 0; y <= 2 * size_y; y++)
{
for(int x = 0; x <= 2 * size_x; x++)
{
// add to current cost
auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y)
+ (double)(safety_grid[x + ((2 * size_x + 1) * y)] * safety_weight);
// detect overflow
if(cost > (int)costmap_2d::LETHAL_OBSTACLE)
{
cost = costmap_2d::LETHAL_OBSTACLE;
}
master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost);
}
}
}
if(use_visibility)
{
// calculate visibility grid
double yaw = tf::getYaw(human.pose.orientation);
auto visibility_grid = createVisibilityGrid(visibility_max, resolution,
costmap_2d::LETHAL_OBSTACLE, cos(yaw), sin(yaw));
if(visibility_grid == NULL)
{
ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as visibility_grid is empty");
return;
}
// apply the visibility grid
auto size_x = (int)(visibility_max / resolution), size_y = size_x;
for(int y = 0; y <= 2 * size_y; y++)
{
for(int x = 0; x <= 2 * size_x; x++)
{
// add to current cost
auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y)
+ (double)(visibility_grid[x + ((2 * size_x + 1) * y)] * visibility_weight);
// detect overflow
if(cost > (int)costmap_2d::LETHAL_OBSTACLE)
{
cost = costmap_2d::LETHAL_OBSTACLE;
}
master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost);
}
}
}
}
// ROS_DEBUG("hanp_layer: updated costs");
}