Fix astar path bug