Some changes to Dijplanner. To do h_function e goal management.