Fix obstacle call functions in solve, to reset outer objects and add objects from the local vector
Consistently throw if planner has been interrupted.