Seeing with lidar

A spinning laser

The rover's lidar sweeps a laser beam all the way round and times each echo. rover.lidar() returns a Scan of 180 beams, one every 2°:

• scan.ranges[i] is how far beam $i$ went before it hit something (m);
• scan.angles[i] is its direction $a$ in the rover's frame: $x$ straight ahead, $y$ to the left, angles anticlockwise from ahead;
• a beam that hits nothing reads exactly scan.max_range (1.2 m). That's a miss, not a wall, and scan.valid is False for it.

From ranges to the world

In the rover's frame, beam $i$ hit the point $(r\cos a,\ r\sin a)$. To put it on the map, rotate it by the rover's heading $\theta$ and shift it by the rover's position $(x, y)$. This rotate-then-shift is the SE(2) transform between the two frames:

Multiplied out, it says that beam $i$ points at $\theta + a$ in the world: $x_w = x + r\cos(\theta + a)$ and $y_w = y + r\sin(\theta + a)$.

Reacting to what you see

Avoiding walls doesn't need a map. A reactive rule turns each scan straight into a command:

• ahead, left and right are the nearest hits within 30° of straight ahead, from 30° to 150°, and from −150° to −30°;
• $v = V_\text{max} \cdot \operatorname{clip}\big((\text{ahead} - \text{STOP}) / \text{SLOW},\ 0,\ 1\big)$: full speed when the way is clear, slowing over the last SLOW metres to a stop STOP metres short of a wall;
• $\omega = K\,(\text{left} - \text{right}) / 0.1$, clipped to $\pm W_\text{max}$: turn $K$ rad/s towards the side with more room for every 10 cm it has over the other. In a corridor that holds the rover in the middle, and at a bend it turns into the opening.

Your task

1. Write scan_to_world(scan, pose): an $(M, 2)$ array of the world points of the $M$ beams that hit, in beam order. Write it yourself rather than calling scan.points().
2. Write react(scan), returning $(v, \omega)$ by the rule above, with the nearest helper.

The program draws your first scan on the walls, then drives the corridor, redrawing the scan every second. The checks want those points within 1.5 cm of the walls, and the rover at the end in under 60 s without a bump.