Remove debug code
Merge pull request #560 from lossyrob/bugfix/shortestpath

Remove debug code