Towards specification, planning and sensor-based control of autonomous underwater intervention