Reimplement Clear Costmaps Service

Fix for #117