Added an algorithm to handle obstacles