Revision history [back]

I think I understand your problem now.

Let's say you have some array of locations in the camera's reference frame that you believe to be free space, according to the camera.

Within your custom layer, you would first need to translate these points into the map's reference frame. This will give you the position of the points in METERS relative to the reference frame.

Then to figure out which grid cells they correspond to in the costmap, for each x and y in the map's reference frame, you'd call worldToMap(x,y,mx,my) to get mx and my, the coordinates of the grid cell in the costmap (integers).

Then, if you wanted to replace LETHAL costs from the obstacle layer with FREE_SPACE, in updateCosts, you should loop through all the points, get the mx and my and then say

if(master_grid.getCost(mx, my)==LETHAL_OBSTACLE){
     master_grid.setCost(mx, my, FREE_SPACE);
}