A warehouse robot keeps an occupancy grid: the floor is cut into square cells res metres wide, and grid[row][col] is 1 if the cell is occupied (a shelf, a wall) and 0 if it is free. To predict what its laser should read, the robot marches along the beam.
The map's frame. The world has x to the right and y up, in metres. The grid's bottom-left corner is at (0, 0), and row 0 is the top row of the map, as on a printed plan. So the point (px, py) lies in
col = floor(px / res)
row = (number of rows - 1) - floor(py / res)
and it is off the map when that col is not in 0 .. cols - 1 or floor(py / res) is not in 0 .. rows - 1 (decide with the computed cell, not with px and py: near an edge, px / res can round up to exactly cols).
The beam. The robot's pose is (x, y, theta), with theta in radians counter-clockwise from +x. The beam starts at (x, y) and samples the points at distances k * step along the heading, for k = 1, 2, ..., n (the k-th sample is at (x + k*step*cos(theta), y + k*step*sin(theta))). At each sample, in order:
- if the point is off the map, the beam is lost: return
(n * step, None); - if its cell is occupied, return
(k * step, (row, col)).
If none of the n samples hits an occupied cell, return (n * step, None): the reading is the sensor's maximum range.
Write grid_beam(grid, res, pose, step, n).
Examples
grid = [[1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 1],
[1, 0, 0, 1, 0, 1],
[1, 0, 0, 0, 0, 1],
[1, 1, 1, 1, 1, 1]]
Input: grid, res = 1, pose = (1.5, 1.5, 0), step = 0.5, n = 20
Output: (3.5, (3, 5))
Explanation: the robot is in cell (3, 1). Samples at x = 2.0, 2.5, ... stay in row 3;
the 7th, at x = 5.0, is in the wall cell (3, 5).
Input: grid, res = 1, pose = (1.5, 1.5, math.atan2(1, 1.5)), step = 0.25, n = 20
Output: (2.0, (2, 3))
Explanation: the 8th sample is at (3.164, 2.609): column 3, and floor(2.609) = 2 gives row 4 - 2 = 2.
Input: grid = [[0, 0, 0, 0], [0, 0, 0, 0], [0, 1, 0, 0]], res = 0.5, pose = (0.25, 0.75, 0), step = 0.2, n = 10
Output: (2.0, None)
Explanation: the map is 2 m wide; the 9th sample, at x = 2.05, is off the map.
Constraints
- the robot's position is on the map, in a free cell
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Turn a world point into the `(row, col)` of the grid cell that contains it
- Sample a beam at even steps and stop at the first occupied cell
- Handle a beam that runs out of range or off the map