adapting servoing to take goal pose, tag id, and image topic while running