Robotics simulation from PNG map

Question

I am a complete beginner at this.

I have an arbitrary map as a PNG file (black and white, only). And I'm supposed to draw a robot at a position(x,y) and then I'm supposed to simulate the robot taking a laser-scan measurement for an opening angle of 130 degrees, separation of each scan-line is 2 degrees. So, obviously: each laser scanline is supposed to be obstructed by black pixels in the PNG map, while the other lines just keep on going for an n distance.

I've attempted drawing scanlines around the object, but I'm running into issues when it comes to getting each line to be obstructed by the black-lines. To my mind, it requires a completely different approach.

Any helpful advice on how to get started would be greatly appreciated.

Answer 1

Using scanlines

A naive approach could be something like this:

for (angle = 0; angle < 360; angle += precision)
    draw_line_from_robot(robot, angle)

which tries to draw many lines from the robot, reaching out to as far as it can. You could use Bresenham's line algorithm for example and as you draw your line, make sure you don't hit a black pixel. When you do, simply stop and try the next angle.

This approach has a couple of problems. Take this map for example:

And the following map with a few lines drawn:

You can see the first problem here. As you get farther from the robot, you get gaps in the scanlines. You could get precision higher, but you could also change your algorithm a bit to make sure you cover every surrounding pixel:

for (i = 0; i < x; ++i)
    draw_line_from_robot_to_destination(robot, i, 0)
    draw_line_from_robot_to_destination(robot, i, y - 1)

for (i = 1; i < y - 1; ++i)
    draw_line_from_robot_to_destination(robot, 0, i)
    draw_line_from_robot_to_destination(robot, x - 1, i)

This way, you are sure to cover all pixels. However, there is a second problem. a lot of pixels close to the robot are being redrawn over and over again.

A better way

Given these limitations (either not covering all pixels, or being highly inefficient), I'm going to propose a different algorithm.

You can use breadth-first search on the pixels of the image (as if it was a graph) starting from the robot. Additionally, keep a binary-search tree of disjoint angle-ranges visible to the robot.

First, let's talk about what the binary-search tree does for you. In the beginning, the tree holds only one node:

                       [0, 360)

which means the robot can see from angle 0 to 360.

Let's say at some point you meet an obstacle, at angle x. You need to block the view of the robot by a certain range, which is blockable by a pixel.

In the picture above, gray pixels are just to show the distance between the robot and the blocked pixel. The green pixel is the robot and the black is the obstacle. The $\alpha$ and $\beta$ are easily calculated. Assuming the block to be at:

$\alpha = arctan(b_x - r_x + 0.5, b_y - r_y - 0.5)$

$\beta = arctan(b_x - r_x - 0.5, b_y - r_y + 0.5)$

The computed, you reduce the blocked range from your tree and get a new tree:

                       [0, a)
                           \
                           (b, 360)

If later you decide to reduce $[\gamma, \delta]$ from your tree, where they are below $\alpha$, you would get:

                       (d, a)
                       /    \
                   [0, g)   (b, 360)

If you want to reduce $[\epsilon, \alpha]$ from the tree, you get:

                       (d, e)
                       /    \
                   [0, g)   (b, 360)

You get the idea!

So what's the final algorithm?

/* this is a BFS */
init_tree(range_tree)
insert_queue(Q, robot_pos)
mark_visited(robot_pos)

while (!empty(Q))
    pos = queue_pop(Q);
    for (all neighbors of pos, n)
        if (n is outside the map or visited before)
            continue
        if (n is black)
            remove_blocked_view(range_tree, robot_pos, n)
        else if (visible_in_ranges(range_tree, robot_pos, n))
            mark_visited(n)
            insert_queue(Q, n)

In the end, the pixels that you have visited are the visible pixels from the robot. In the above algorithm, remove_blocked_view is the algorithm I described above, and visible_in_ranges is a simply binary-search tree lookup that tells whether the angle from robot_pos to n ($arctan(n_x - r_x, n_y - r_y)$) is in the tree or not. If it is in the tree, the pixel is visible. If it is not, then it's not visible.

Answer 2

If I understand the problem correctly, this is very straightforward. Given a set of grid points that can be either filled or unfilled, a point of origin, and a set of scan angles, you want to find the distance of the closest filled point for any scan angle.

To do this optimally, you will need to find an algorithm that can give you the full set of points that would be covered by your scan angles, up to distance n. For now, we can skip that -- the rest of the algorithm is the same.

x_o, y_o    = get_origin_point()
theta_o     = get_center_angle_of_desired_scan_direction()
n           = get_max_sensor_distance()
sweep_angle = 130
beam_width  = 2   
num_bins    = sweep_angle / beam_width

# initialize the array of scanned lines to the maximum sensor distance
scan_results = []
for angle in range(bins):
    scan_results[angle] = n

# iterate over all points in the grid
for (x, y) in all_points:

    if not point_is_filled(x, y): continue

    # distance from scanner
    r = math.hypot(x - x_o, y - y_o)     
    if n < r: continue

    # the angle of this point relative to scanner
    theta = math.atan2(y - y_o, x - x_o) 

    # the corresponding scan line bin this angle is in
    bin = (theta - theta_o - (sweep_angle / 2)) / beam_width
    if bin < 0 or num_bins <= bin: continue

    # update value if this point is closer than what was already sensed
    scan_results[bin] = min(scan_results[bin], r)