Understanding costmap_2d tutorial: Updating master_grid
I'm trying to understand this tutorial about the new layered costmap structure in Hydro. I successfully integrated the grid_layer and it runs out of the box just fine.
I then started to work with a recorded bag-file and the used layer structure in the yaml file looks like
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: grid_layer, type: "navigation::GridLayer"}
So to my understanding the static_layer should be the "lowest" layer. Then comes the obstacle_layer handling the input data and then the new grid_layer (according to this paper). So there are two rounds: In one round all layers update the Bounds to their needs. Then in the second round each layer writes to the master costmap (master_grid), right? So now the critical part of the code I don't understand is the following:
void GridLayer::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]);
}
}
}
In the master_grid I clearly see obstacles moving around since the obstacle layer updates the master_grid as wished for. But then when its the grid_layer's turn to read and write the master_grid, the whole master_grid seems to contain NO_INFORMATION values (except for that tracking dot of course). Am I missing sth? Why does the grid_layer not see the obstacles updated by the obstacle_layer to the master_grid?
Is there an option in the yaml file I need to adjust or is it sth in the code?
Thanks for your time.
The master grid should change, as you expect. Can you post your costmap configuration?
It does change. What I wanted to do was updating the layered_costmap_ (called master costmap in the paper). The way I would do it now is updating the master_grid as mentioned above and then use the functions updateWithOverwrite or updateWithMax. But I have not found time to try that out yet.
If you'd still like help with this, please post the configuration.
Thanks for offering your help. I think the answer I posted below solved the problem to a degree that I could continue with the construction of my own layer. My configuration is the same as in my other post: http://answers.ros.org/question/165571/obstacle_layer-does-not-clear-area/