A try to create dijkstra with more origins. Not working till now.
Merge branch 'master' of https://github.com/NemanjaFilipovic/garbage_truck_automation