Add more test for the astar algorithm