AGVCGoal provider is now fully operational. A few bug fixes for goal_provider as well