U9.8 Roadmaps

Build a roadmap once, then drive three queries on it without touching a wall: from the start to the green corner, from there to the bottom right bay, and from there to a third goal that nobody knows until the program runs. - The build. 150 free samples, each linked to its K = 8 nearest neighbours that clear() says it can see. Only samples: the start and the goals are not in it. Straight after the build, print nodes: and how many nodes the roadmap has, which is 150. - add_node(p). Takes a point p = (x, y) in mat centimetres, adds it to the roadmap linked to its K nearest nodes that it can see, and returns its node number. - query(a, b). Takes two points, adds both with add_node, and runs Dijkstra between them. Returns (route, length), where route is the list of (x, y) points from a to b and length is its length in centimetres, or None if the search never reaches b. When a query comes back None, throw the samples away, build a fresh roadmap of 150 and ask again. - The third goal. The simulator picks it afresh every run, somewhere in the free space to the left of the first wall and at least 30 cm from where the robot starts, and the program reads it with input(): two lines, x then y, in mat centimetres. The starter reads it into GOAL3, and the mat shows it as a red square once the run starts. It is not in the build either; it joins the roadmap like any other query point. - The trips. Visit the corner, then the bay, then finish within 8 cm of GOAL3, one query each. Start each query from here(), where the robot actually is. Before driving each one, print query 1 length:, query 2 length: and query 3 length: with the route's length. free(), clear(), here(), GOAL3 and follow(route), which drives a list of waypoints the way U9.7 did, are written for you.

from bugbot import *
import heapq, math, random
connect()

DT = 0.1
INFLATE = 10.0
N_SAMPLES, K = 150, 8
V_MAX, V_LAT = 20.0, 15.0
START = (30.0, 30.0)
GOALS = [(170.0, 170.0), (170.0, 30.0)]        # the corner, then the bay
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

def clear(a, b):
    d = math.hypot(b[0] - a[0], b[1] - a[1])
    n = max(2, int(d / 1.5))
    return all(free(a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n) for k in range(n + 1))

GOAL3 = (float(input()), float(input()))       # the third goal: the simulator picks it every run, x then y in mat cm

def here():
    px, py = position()
    return START[0] + px, START[1] + py

def follow(route):
    """Drive a list of (x, y) waypoints in order, holding the heading at zero."""
    for wx, wy in route[1:]:
        for tick in range(400):
            x, y = here()
            dx, dy = wx - x, wy - y
            gap = math.hypot(dx, dy)
            if gap < 4.0:
                break
            speed = min(13.0, 3.0 + 0.5 * gap)
            vx, vy = speed * dx / gap, speed * dy / gap
            h = math.radians(heading())
            spin = (heading() + 180) % 360 - 180
            drive(100 * (vx * math.sin(h) + vy * math.cos(h)) / V_MAX, 100 * (vx * math.cos(h) - vy * math.sin(h)) / V_LAT,
                  max(-30.0, min(30.0, -0.8 * spin)))
            wait(DT)
    stop()

# build the roadmap once from samples alone, then query it three times

Plan your program here, then type it in and press Run.

Do it on the robot
www.bugbotlab.com/learn/u9-8-roadmaps/
The simulator checks it and tells you when it passes. Nothing to install, no account.