SHORTEST_PATH: Added vertices and shortest path algorithm

- Vertices added. Adding walls breaks connections and removing
  walls renews the broken connections.
- Shortest path is now calculated with Dijkstra's algorithm
  (in calcPath function) and the path_ is stored in IRoomba. The
  function's return value is the length of the path.
-  Adding/removing walls seems to work with vertices but the
   calcPath is untested