Clearing free space in costmap at a specific location
I'm trying to remove obstacles that are being detected with the laser and mark them as free space. Using a layer I am able to create obstacles that are included in the global costmap, but I don't seem to be able to remove them completely. I am guessing this is because of how the layers are merged - any lethal obstacle being added to the final costmap. Is there any way I can remove obstacles from the obstacle layer, or force free space from my layer to be dominant in the merging of layers?
xml file
<class_libraries>
<library path="lib/libcustom_layer">
<class type="custom_cost_namespace::CustomLayer" base_class_type="costmap_2d::Layer">
<description>Obstacle adding and removing based on labels </description>
</class>
</library>
</class_libraries>
yaml file
plugins:
- {name: static_layer, type: 'costmap_2d::StaticLayer'}
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
- {name: custom_cost, type: 'custom_cost_namespace::CustomLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
publish_frequency: 1.0
footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
launch file
<rosparam file="$(find custom_cost)/config/custom_layer.yaml" command="load" ns="global_costmap" />
update costs method
void CustomLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
min_j, int max_i, int max_j)
{
if (!enabled_)
return;
if(master_grid.worldToMap(x, y, mx, my))
{
if (remove)
master_grid.setCost(mx, my, FREE_SPACE);
else
master_grid.setCost(mx, my, LETHAL_OBSTACLE);
}
}
Can you post your costmap configuration?
Hi David, I have added the xml and yaml files. Is that all you need?
How does your CustomLayer look like? Did you just copy some stuff from another layer? In the updateCost() function you can specify how the layer should update the final / master costmap. The update area normally is the same for all layers (parameters of updateCosts)
I have added my updateCosts function. I did just use the tutorial as an example.
With an additional layer, you can block the obstacle layer's lethal values from staying in the master costmap. However, it will not remove the value from the obstacle layer.
So would the configuration above block the obstacle layer? If so, could it be that it is not happening at the correct location? I have an x,y already in the map frame, and am concerned that the worldToMap might not be appropriate.
The tutorial layer does not save anything related to clearing so it does not overwrite the obstacle layer. Do you really need a custom layer? It would be easier if you just added the laser as observation source to the obstacle layer. This way the laser could clear thing in the obstacle layer.
You could make it kind of overwriting with commenting the lines 71 and 72 out in the custom layer cpp (the if (costmap_[index] == NO_INFORMATION) continue; thing). But this probably won't do what you actually want (if you have more than one sensor).
I am using the camera to determine if objects the laser believes to be obstacles are in fact traversable. So the decision to update the cost is coming from another image processing node. Perhaps I need to update the obstacle layer directly? If so, then how?
In your updateBounds function add the places you want to mark as free with setCost(mx, my, costmap_2d::NO_INFORMATION). I think that should do the trick. You still will be working with two different layers though. Obstacle layer will add stuff, and custom will overwrite it every time.
The updateBounds function does not pass in the master grid like updateCosts does. How can I access the grid in this function?