Understanding costmap_2d tutorial: Updating master_grid

Question

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.

---

Originally posted by Luke_ROS on ROS Answers with karma: 116 on 2014-04-30

Post score: 0

---

Original comments

Comment by David Lu on 2014-05-14:
The master grid should change, as you expect. Can you post your costmap configuration?

Comment by Luke_ROS on 2014-05-15:
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.

Comment by David Lu on 2014-05-19:
If you'd still like help with this, please post the configuration.

Comment by Luke_ROS on 2014-05-20:
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/

Answer 1

I'm not quite sure but I think I found part of the answer. My mistake was to assume that the master_grid would refer to the master costmap as mentioned in the paper. But this does not seem to be the case. If I would like to access the master costmap I should go over the so called layered_costmap_

Even though this may be the ugliest code around this is how I am now able to read out of the master costmap:

// READ
costmap_2d::Costmap2D* layered_costmap = layered_costmap_->getCostmap();
unsigned char* master_array = layered_costmap->getCharMap();
unsigned char element_500 = master_array[500];

But I am still confused how to properly write to the master costmap (layered_costmap_)... Do I go via the master_grid? Or do I go over functions like updateWithOverwrite()? Or may it be sth into the direction of layered_costmap->setCost(x,y,value)?

---

Originally posted by Luke_ROS with karma: 116 on 2014-05-03

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by matteopantano on 2017-05-05:
I am facing the same stuff, did u got something?