Added handling of obstacle goals