Started new functions on DijPlannerEvo