Fixing current_goal