Added Dijkstra function to DijPlannerEvo with logging