stop sleeping and logging every time
adding costmaps to the logging when its the first nav failure
Merge pull request #37 from bfalacerda/hydro-devel

adding costmaps to the logging when its the first nav failure