Project: plan the route, then drive it

Choose and justify a route planner, build the mat's graph, find the shortest route with Dijkstra's algorithm and drive it.

A5.9Algorithms and complexityA level35 min

This project puts the module together on the mat. BugBot starts at waypoint S and has to reach waypoint G along the roads between waypoints, without touching a wall. You will choose a planning algorithm and justify it, build the graph from the map, run Dijkstra's algorithm, and drive the route it finds.

The map as data

The robot's map is two pieces of data. WAYPOINTS gives each waypoint's position on the mat in cm, with x across and y up the mat from the bottom-left corner. ROADS lists which pairs of waypoints are joined by a straight road the robot can drive along safely. Every road is either along x or along y, so the robot never has to turn: it drives forward, backward, left and right.

import math

WAYPOINTS = {"S": (10, 10), "A": (10, 90), "B": (90, 90), "C": (35, 10), "D": (35, 35),
             "E": (65, 35), "F": (65, 60), "G": (90, 60), "J": (80, 35)}
ROADS = [("S", "A"), ("A", "B"), ("B", "G"), ("S", "C"), ("C", "D"),
         ("D", "E"), ("E", "F"), ("F", "G"), ("E", "J")]

for a, b in ROADS:
    print(a, "to", b, math.dist(WAYPOINTS[a], WAYPOINTS[b]), "cm")

Run this in the simulator

A road's weight is its length, the straight-line distance between its ends, which math.dist works out. Roads can be driven both ways, so the graph is undirected: each road goes into the adjacency list of both its ends.

Choosing the algorithm

Before writing code, think about what each approach you have met would do on this map.

Try every route. Listing every route from S to G and taking the shortest always works, but the number of routes can grow factorially with the number of waypoints (A5.1). Nine waypoints is fine; a real building with a few hundred is not.

Greedy: always take the nearest next waypoint. From S the nearest is C (25 cm), then D (25), then E (30). From E the nearest is J, only 15 cm away, and J is a dead end. A greedy rule is fast but can walk into a trap, because it never looks further than one road ahead.

Breadth-first search (A4). It finds the route with the fewest roads: S, A, B, G, only 3 roads but 80 + 80 + 30 = 190 cm. The route through the middle uses 5 roads and is shorter. Fewest roads is not shortest distance when roads have different lengths.

Dijkstra's algorithm (A5.7). Finds the shortest total distance, and works because no road has a negative length. With V waypoints and E roads it is O((V + E) log V) with a priority queue, so it would still be fast on a map of thousands of waypoints.

A* (A5.8). Also correct here, with straight-line distance to G as an admissible heuristic, and it would expand fewer waypoints. On a map this small the saving is a handful of vertices, and Dijkstra's algorithm also gives the distance to every waypoint, which is handy if the robot is later sent somewhere else. Either is a good answer if you justify it.

This project uses Dijkstra's algorithm.

Driving a route accurately

The robot drifts a little on every move (module 2). Driving five roads in a row, the errors add up. The starter includes a helper, go_to(x, y), that reads position() before each move and drives the difference, so each road corrects the error left by the one before. position() measures from where the robot started, so the helper adds the start's mat position to it. This cell runs on the project's mat: it drives from S to D and back.

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

START = (10, 10)                    # the robot starts at waypoint S

def go_to(x, y):
    """Drive to the mat point (x, y): along y, then along x, twice so the second pass corrects any drift."""
    for attempt in range(2):
        px, py = position()
        dy = y - (START[1] + py)
        if dy > 1:
            forward(60, distance=dy)
        elif dy < -1:
            backward(60, distance=-dy)
        px, py = position()
        dx = x - (START[0] + px)
        if dx > 1:
            right(60, distance=dx)
        elif dx < -1:
            left(60, distance=-dx)

for point in [(35, 10), (35, 35), (35, 10), (10, 10)]:      # S to C to D, and back
    go_to(*point)
    px, py = position()
    print("aimed for", point, "reached", (START[0] + px, START[1] + py))

Run this in the simulator

Putting it together

Your program has four parts, each one a separate job:

1. Build the graph: a dictionary from each waypoint to a dictionary of its neighbours and road lengths.
2. Plan: dijkstra(graph, start) returns the distances and previous waypoints.
3. Read the route back from G to S through the previous waypoints, and reverse it.
4. Drive: go_to each waypoint on the route in order, then signal arrival.

Keeping planning and driving apart is good design. The planner can be tested with no robot at all, and the same driving code will follow whatever route any planner produces.

Task: plan the route, then drive it

The starter has the map and the go_to helper. Write the rest:

• Build an undirected weighted graph from WAYPOINTS and ROADS, where each road's weight is its length in cm (use math.dist; do not type the lengths).
• Write dijkstra(graph, start) and use it to find the shortest route from S to G. The program must find the route: do not type it.
• Print route: <waypoints joined with ->, such as route: S-X-Y-G, and length: <n> cm, with n a whole number.
• Drive the route by calling go_to for each waypoint after S, in order. Stay on the roads: do not go near waypoints that are not on your route, and do not touch a wall.
• When the robot reaches G, turn the LED green.

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

import math
import heapq

WAYPOINTS = {"S": (10, 10), "A": (10, 90), "B": (90, 90), "C": (35, 10), "D": (35, 35),
             "E": (65, 35), "F": (65, 60), "G": (90, 60), "J": (80, 35)}
ROADS = [("S", "A"), ("A", "B"), ("B", "G"), ("S", "C"), ("C", "D"),
         ("D", "E"), ("E", "F"), ("F", "G"), ("E", "J")]
START = WAYPOINTS["S"]              # the robot starts at S

def go_to(x, y):
    """Drive to the mat point (x, y): along y, then along x, twice so the second pass corrects any drift."""
    for attempt in range(2):
        px, py = position()
        dy = y - (START[1] + py)
        if dy > 1:
            forward(60, distance=dy)
        elif dy < -1:
            backward(60, distance=-dy)
        px, py = position()
        dx = x - (START[0] + px)
        if dx > 1:
            right(60, distance=dx)
        elif dx < -1:
            left(60, distance=-dx)

Challenges

1. The road from E to F is closed for repairs. Remove it from ROADS and plan again. What route and length do you get, and would the robot hit a wall driving it?
2. Add A* with straight-line distance as the heuristic, and count how many waypoints each algorithm expands before G.
3. Add a second destination and use the distances from one run of dijkstra to decide which destination is nearer, without planning twice.