Added new getters, reorganized code
Optimized edge creation, added vertex_map
Added RoutePlanner class
Routing test