Adaptive control of AUVs using onboard planning and execution