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 trips. 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. - The third goal. Once the second trip is done, pick a random free point anywhere on the mat (free() says whether a point is free), print it as goal 3: x, y, then query the roadmap you already have and drive there. Leave the random module unseeded, so the third goal is somewhere new every run. free(), clear(), here() 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))

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.