follow_planner: add logic to follow a moving goal