Astar planning implementation