SHORTEST_PATH: Roomba follows shortest path around walls

- finding shortest path with walls in the way works
- removing walls correctly renews connections (not thorougly
  tested)
- roomba follows the path correctly
- currently there are simple lines showing the route to help in
  future debugging
- checkPoiCollision doesn't remove poi to avoid a crash. A more
  clever implementation of the func will avoid the crash