Added check_single_goal to DijPlanner