Don't plan approach if busy executing motion.

- Implementation was broken in the sense that it was always computing approach
  trajectories, even when the request was to be rejected.

- Refactored how controllers are checked, so that the busy state is detected at
  the earliest possible moment.