Programming for Robotics 1, Chapter 3: Challenge 1 solution (tests for the bump sensor)

Idea: a simulated test is "put the robot somewhere, drive it, assert about the result". All three tests share one helper that drives at a fixed duty until the world reports a contact (sim.contact is not None), and then look at three different things.
    (a) Front wall.    bump() must be true. Then keep commanding the same duty for one more second and check that the position barely changes: the collision code undoes every step that would overlap the wall, so the robot may creep the last millimetre or two but never go through. The body must end up touching: the gap 3.0 - x - body_radius is between 0 and 3 mm (one 5 ms physics step at 0.24 m/s is 1.2 mm).
    (b) Rear wall.     The robot reverses into the west wall at x = 0. The contact exists (sim.contact is not None) but the contact point is behind the robot, 180 degrees from the heading, outside the front half-circle of the default bumper, so bump() is False. A bumper with arc_rad = 2 pi covers the whole body, so the same contact now presses it. This is the test that catches a bumper that reports every contact whatever the direction.
    (c) Post to the left.   The robot's disc (radius 0.09 m) touches a post of radius 0.10 m when the centres are 0.19 m apart. The post is 0.12 m to the left of the path, so at contact the centre of the post is  sqrt(0.19^2 - 0.12^2) = sqrt(0.0217) = 0.1473 m ahead and 0.12 m to the left:
            bearing = atan2(0.12, 0.1473) = 39.2 degrees to the left of the heading.
        The simulator never sees that number: it only knows the normal (from the post's centre to the robot's centre), so the bearing is  atan2(-ny, -nx) - theta, the direction of the contact point seen from the robot, which is what BumpSensor.pressed does. The test compares the two to one degree (a step can end up to 2 mm inside, which moves the bearing by a fraction of a degree).

CODE:
arena = World(3.0, 2.0)                                # the wall ahead of an east-facing robot is at x = 3.0


def drive_until_bump(sim, duty, max_s=10.0, dt_s=0.02):
    """Drive at a fixed duty until the world reports a contact; returns the time, or None."""
    for _ in range(int(max_s / dt_s)):
        sim.set_duty(duty, duty)
        sim.advance(dt_s)
        if sim.contact is not None:
            return sim.now_s()
    return None


def test_front_bump_stops_the_robot():
    sim = Simulator(arena, pose=(2.0, 1.0, 0.0))
    t = drive_until_bump(sim, 0.6)
    assert t is not None, "the robot never reached the wall"
    assert sim.bump(), "a wall dead ahead must press the front bumper"
    x_at_bump = sim.pose()[0]
    for _ in range(50):                                # keep pushing for one more second
        sim.set_duty(0.6, 0.6)
        sim.advance(0.02)
    assert abs(sim.pose()[0] - x_at_bump) < 0.003, "the robot drove into the wall"
    gap_m = 3.0 - sim.pose()[0] - sim.params.body_radius_m
    assert -1e-9 <= gap_m < 0.003, "the body should end up touching the wall"
    return t, gap_m


def test_rear_contact_is_not_a_front_bump():
    sim = Simulator(arena, pose=(0.5, 1.0, 0.0))       # facing east, reversing into the west wall (x = 0)
    t = drive_until_bump(sim, -0.6)
    assert t is not None and sim.contact is not None, "the robot never touched the wall behind it"
    assert not sim.bump(), "a front bumper must not report a contact at the back"
    sim.bumper = BumpSensor(arc_rad=2 * math.pi)       # a bumper all the way round
    assert sim.bump(), "a full ring must report it"
    return t


def test_contact_bearing_of_a_post():
    post = World(3.0, 2.0)
    post.add_circle(1.5, 1.12, 0.10)                   # a post 0.12 m to the left of the robot's path
    sim = Simulator(post, pose=(0.5, 1.0, 0.0))
    drive_until_bump(sim, 0.6)
    nx, ny, depth = sim.contact
    bearing = math.degrees(wrap_pi(math.atan2(-ny, -nx) - sim.model.theta))
    expected = math.degrees(math.atan2(0.12, math.sqrt(0.19 ** 2 - 0.12 ** 2)))     # centres 0.09 + 0.10 apart
    assert abs(bearing - expected) < 1.0, "the contact bearing is wrong"
    return bearing, expected


t, gap = test_front_bump_stops_the_robot()
print("front wall: bump at t = %.2f s, then held still; body gap %.3f mm" % (t, gap * 1000))
print("rear wall: contact at t = %.2f s, front bumper silent, full ring pressed" % test_rear_contact_is_not_a_front_bump())
bearing, expected = test_contact_bearing_of_a_post()
print("post to the left: contact bearing %.1f deg (geometry says %.1f deg), inside the front arc: yes" % (bearing, expected))
print("all bump tests passed")
END CODE
OUTPUT:
front wall: bump at t = 3.88 s, then held still; body gap 0.009 mm
rear wall: contact at t = 1.80 s, front bumper silent, full ring pressed
post to the left: contact bearing 39.3 deg (geometry says 39.2 deg), inside the front arc: yes
all bump tests passed
END OUTPUT

Reading the output: the front-wall bump comes at 3.88 s and the body ends 0.009 mm from the wall, touching to better than a hundredth of a millimetre; the rear test shows the contact, the silent front bumper and the pressed full ring; the post test finds 39.3 degrees against the 39.2 of the geometry.

Check on method: the three ways to get this wrong are (1) testing only "the robot stopped", which a robot that stopped for another reason also passes (the gap and the bump flag are the real evidence), (2) forgetting that the bump flag is reported only for the last advance() call, so it must be read right after the call and not after a later one, and (3) trusting the normal's sign: it points from the obstacle to the robot, so the contact point is the opposite direction, which is why the formula has minus signs. A test that drives the same scenario at two different headings would catch (3) even if the first worked by accident.