Refactored SubgoalPathFinder to use AStarSearch