adding costmaps to the logging when its the first nav failure

for debugging purposes. needs https://github.com/strands-project/strands_movebase/pull/33 and https://github.com/strands-project/strands_navigation/pull/171