Path planning with Dijkstra's algorithm