Added detect_obstacle function using sonars.