Optimize stop in case of obstacle