catching costmaps exceptions

I think this is ok but I dont have a running system to test it atm. Please test before merging.