Remove coordinates on the searching path with an obstacle obstruction

[patlai]

if the robot is travelling along its searching path and there is an obstacle on top or near one of the coordinates, the robot is currently doing obstacle avoidance and going backwards again to try to reach that point. As a result, it is stuck in an infinite loop of switching between navigation and obstacle avoidance. The point should be removed from the array of waypoints to modify the searching path