Added aStar path search for robot wp navigation.