Recovery behaviors / costmap cleanup

[jubeira]

The local costmap isn't cleaned quite well yet. E.g. in this case, the laser can see through the obstacles, but they are not cleared in the local costmap. Recovery behaviors also need some testing and tuneup.