Fix some bugs in ARAStar search

The search now does not clean close set in every iteration. This change
aims to maintain m_cameFrom in legal order