A delivery robot reverses into loading bays using an ultrasonic sonar. Unlike a laser, a sonar does not send a thin beam: its sound spreads in a cone, and it reports the distance of the nearest thing anywhere inside that cone, without saying where in the cone it was.
The robot's pose is (x, y, theta): its position in metres (x to the right, y up) and the direction the sonar faces, theta, in radians counter-clockwise from the +x axis. The cone has half-angle half_angle: a point is inside it when the angle between the facing direction and the direction from the robot to the point is at most half_angle (either side). Angles that differ by a whole turn of 2\pi are the same direction, so wrap the difference into (-\pi, \pi] before you compare.
points lists the obstacle points (px, py) the sonar could hear. Write sonar_range(pose, half_angle, points, max_range) that returns the distance from the robot to the nearest point inside the cone, or max_range if no point inside the cone is at most max_range away.
Examples
Input: pose = (0, 0, 0), half_angle = 0.25, points = [(3, 0.5), (2, 1.5), (4, -0.2)], max_range = 6
Output: 3.0413812651491097
Explanation: (3, 0.5) lies 0.165 rad off the axis, inside the cone, 3.041 m away.
(2, 1.5) is closer (2.5 m) but 0.644 rad off the axis, outside. (4, -0.2) is inside but further.
Input: pose = (0, 0, 3.0), half_angle = 0.25, points = [(-5, -0.5), (1, 0)], max_range = 6
Output: 5.024937810560445
Explanation: the direction to (-5, -0.5) is -3.042 rad; minus the heading 3.0 that is -6.042,
which wraps to 0.241 rad: inside the cone. (1, 0) is behind the robot.
Input: pose = (1, 2, math.pi / 2), half_angle = 0.3, points = [(1, 9), (3, 3)], max_range = 5
Output: 5.0
Explanation: (1, 9) is straight ahead but 7 m away; (3, 3) is far off to the right.
Constraints
- no point coincides with the robot's position, and none lies exactly on the edge of the cone
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Decide whether a point lies inside a sensor's cone from its bearing
- Wrap the difference between two angles before comparing it
- Report the nearest echo, or the maximum range when there is none