NavFn: fixed #26, planned paths brush obstacles.
navfn: debugging help for previous commit fixing #26.
This commit is low priority, it is just all the debugging infrastructure I used when
researching the fix for #26.  It doesn't change anything for normal operation.
Merge branch 'groovy-devel' of github.com:ros-planning/navigation into groovy-devel