Reactive mission and motion planning with deadlock resolution avoiding dynamic obstacles