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).