Building Routable Road-Network Graphs from HD Maps

A planner does not route on lane polylines — it routes on a weighted directed graph, and the quality of that graph decides whether the vehicle can find a legal path at all. Building it from an HD map means turning the lane geometry and lane-level topology into nodes and edges with the right granularity, the right edge types, and cost weights that make legal, comfortable paths cheap. This sits at the routing end of the lane geometry extraction and road network processing domain, downstream of centerline and attribute extraction. The failure that matters most is an unreachable island of lanes, so the whole construction is gated on a network-wide reachability proof.

Lane geometry and topology become a weighted graph with three edge types, gated on reachability:

Representation Overview #

The core modelling choice is node granularity, and it trades graph size against the fidelity of the lane-change model:

Lane-as-node is compact but cannot express a lane change that begins partway along a lane; lane-segment-as-node splits each lane at a fixed step so lateral edges can attach mid-lane. Waypoint granularity is rarely used for network routing — it belongs to the local planner. Most production routers use lane-segment nodes with the step sized to the shortest realistic lane-change distance.

Stage-by-Stage Implementation #

Stage 1 — Emit nodes at the chosen granularity #

Split each lane centerline into arc-length segments and create a node per segment. Carry the lane id and the arc-length interval so edges can be attached precisely.

import networkx as nx
import numpy as np

def add_segment_nodes(g: nx.DiGraph, lane_id, centerline: np.ndarray, step=15.0):
    """One node per `step`-metre slice of the lane."""
    s = np.concatenate([[0.0], np.cumsum(np.linalg.norm(np.diff(centerline, axis=0), axis=1))])
    breaks = np.arange(0.0, s[-1], step)
    for i, s0 in enumerate(breaks):
        g.add_node((lane_id, i), lane=lane_id, s_start=float(s0),
                   s_end=float(min(s0 + step, s[-1])))

Key parameter: step is the segment length — smaller steps allow finer lane-change attachment at the cost of a bigger graph.

Stage 2 — Add the three edge types #

Successor edges run along a lane and across successor links; lane-change edges connect laterally adjacent segments where a change is legal; turn edges cross junctions from the lane graph.

def add_edges(g, lane_graph, adjacency, legal_change):
    # longitudinal successors within and across lanes
    for u, v in lane_graph.edges:
        g.add_edge(u, v, kind="successor", base=_length(g, u))
    # lateral lane changes where legal
    for a, b in adjacency:            # laterally adjacent segment pairs
        if legal_change(a, b):
            g.add_edge(a, b, kind="lane_change", base=_length(g, a))

Key parameter: legal_change consults lane markings and regulatory attributes — a lane change is added only where the map permits it, never from geometric adjacency alone.

Stage 3 — Assign edge costs #

Weight each edge so routing prefers legal, comfortable paths. Cost combines physical length with penalties that discourage unnecessary turns and lane changes.

def edge_cost(kind, length, turn_penalty=8.0, change_penalty=20.0):
    c = length
    if kind == "turn":
        c += turn_penalty
    elif kind == "lane_change":
        c += change_penalty
    return c

Key parameters: turn_penalty and change_penalty are additive costs in metre-equivalents; raising change_penalty makes the router hold a lane longer before changing.

Validation & QC Automation #

The decisive gate is reachability. Also validate that costs are positive and every node has consistent degree.

• Reachability: every drivable lane node is in the same weakly connected component as the network core; isolated components are flagged with their tile ids.
• No zero/negative costs: every edge weight > 0 so shortest-path algorithms terminate correctly.
• Turn coverage: every junction in the lane successor graph produced at least one turn edge.

def assert_reachable(g: nx.DiGraph):
    comps = list(nx.weakly_connected_components(g))
    comps.sort(key=len, reverse=True)
    isolated = [c for c in comps[1:] if len(c) < 3]  # small islands
    assert not isolated, f"{len(isolated)} isolated lane island(s)"
    assert all(d["base"] > 0 for *_, d in g.edges(data=True)), "non-positive cost"

The granularity question decided by what each choice can and cannot express. One node per lane is smaller and cannot say where along the lane a change is legal; one node per station can, and pays for it:

Edge Cases & Failure Patterns #

• Tile-border islands. A lane whose successor lives in an unloaded neighbouring tile looks isolated. Stitch cross-tile links before the reachability check, using the boundary handling from managing map tile boundaries in ROS2.
• Illegal lane changes from geometry. Adding lateral edges from adjacency alone lets the router cross a solid line. Gate every lane-change edge on the marking/regulatory attribute.
• Negative-cycle from bad costs. A mis-signed penalty can create a zero or negative edge, breaking Dijkstra. Assert strictly positive weights.
• Over-penalized turns. Turn penalties set too high make the router take absurd detours to avoid a legal turn. Calibrate penalties against known good routes.

The three edge types are not interchangeable, and cost has to be assigned per type or the planner will happily route a vehicle through six lane changes to save two metres:

Performance & Scale Notes #

Segment granularity is the dominant cost driver: halving step roughly doubles node and edge counts. Choose the largest step that still supports the planner's lane-change model. Build per tile and stitch at borders so the whole-map graph never has to be resident at once, and store it in a compressed sparse form for the router. Reachability analysis is a single linear-time connected-components pass; run it after stitching. The construction parallelizes cleanly per tile — the same batch pattern used across the lane geometry extraction domain.

Whichever granularity is chosen, the three edge types must stay distinguishable on the edge itself. A successor edge follows a lane forward; a lane-change edge crosses sideways and only where the marking permits; a junction edge crosses a junction interior expanded from the connection table. They differ in what they cost and in what constrains them, and an edge that does not carry its type is an edge no cost function can price.

Node granularity, decided once #

Before any of the above, one modelling decision fixes what the graph can express: whether a lane is one node or many.

One node per lane is compact and cannot say where along the lane something is true. A lane-change edge under that model is legal for the whole lane or not at all, which cannot express the common case of a solid line for the first sixty metres and a dashed line after it. One node per station — typically every 20 m — can express it, at roughly five times the node count and a correspondingly larger routing search.

The workable answer is neither uniform choice but a split at the points where an attribute changes: a lane is one node until a marking, a speed limit or a restriction changes partway along it, at which point it becomes two. That keeps the node count close to the lane count on the majority of the network, where nothing changes mid-lane, and pays the extra node only where the map has something to say.

Junction interiors belong in the graph #

The graph as described so far routes cleanly along roads and stops at junctions unless the interior is expanded into it. The tempting shortcut — one edge per movement, straight from incoming lane to outgoing lane — treats the junction as a hop, and it discards three things at once: the interior length, so route costs are wrong; the turn's curvature, so a planner cannot tell a sweeping right from a hairpin left; and any place to record that two movements cross.

Expanding properly costs one node per connecting-road lane and gives all three back. The interior path becomes a first-class edge pair with its own length and curvature, and a conflict — a place where two turn paths cross, so two vehicles cannot both be there — attaches to it as an annotation with a station along each path. Conflicts must be annotations rather than edges: nobody drives a conflict, so making it an edge corrupts every routing query with paths that cross from one turn onto another.

The check that makes the expansion trustworthy is reachability. A missing laneLink row, a mis-signed connecting lane or a dropped connection record all produce a graph that is structurally valid and in which some approach can no longer reach some exit. Node and edge counts are unchanged, every individual edge is correct, and the only symptom is a legal movement that has become unroutable — which nothing local will find.

Costing edges so the route is the one a driver would take #

Correct topology still routes badly if edges are costed by length. A distance-only graph will take six lane changes to save two metres, cut through a conflicted left turn to avoid fifty metres of straight road, and prefer a service road because it is shorter.

The fix is to cost in time, because every other term the route needs to weigh is naturally expressed in seconds. A lane change is worth some number of seconds of detour; a conflicted turn is worth some number of seconds of waiting. In distance those become constants with no unit, and nobody can say whether 25 is too much. In seconds they are estimates that can be measured from fleet behaviour and argued about on the evidence — a 1.5 s lane-change penalty at 25 m/s is about 38 m of equivalent detour, which is roughly what a driver will accept.

Four terms cover the cases that matter: traversal time from length over speed limit; a flat manoeuvre penalty per lane change or turn; a conflict-exposure charge proportional to how many movements a junction interior crosses; and a comfort term for lateral acceleration above a threshold, derived from the curvature the edge already carries. Adding them in order on a fixed origin-destination pair lengthens the route by about seven per cent and stops it doing three things a passenger would object to.

One property must survive all of it: if the graph is searched with A*, the heuristic has to stay admissible. Straight-line distance divided by the network's maximum speed never over-estimates; using a local speed limit makes it optimistic in slow areas and pessimistic in fast ones, and the pessimistic half silently costs optimality — the search still returns a route, it just occasionally returns a worse one, with nothing to indicate why.

FAQ #

Should a lane be one node or many? #

It depends on where lane changes are allowed. Lane-as-node is compact and fine when lane changes only happen at lane boundaries, but it cannot express a change that begins partway along a lane. Lane-segment-as-node splits each lane at regular arc-length intervals so lateral edges can connect mid-lane, at the cost of a larger graph. Most planners that model continuous lane changes use the segment representation.

How are lane-change edges different from successor edges? #

A successor edge is longitudinal: it connects the end of a lane to the start of the next lane along the direction of travel. A lane-change edge is lateral: it connects a point on one lane to a laterally adjacent point on a parallel lane in the same direction. Lateral edges carry a higher cost and are only added where a change is legal, which the lane markings and regulatory attributes determine, not geometry alone.

Why validate reachability separately from topology? #

Topology validation confirms links are well-formed; reachability confirms the graph is usable. A map can have perfectly valid links yet contain an island — a pocket of lanes connected to each other but not to the rest of the network, often a mapping gap at a tile border. Only a connectivity analysis over the whole graph finds those islands.

Related #

• Converting Lanelet2 Maps to Routing Graphs — the concrete Lanelet2-to-graph implementation of this workflow.
• Building Lane Successor Graphs from OpenDRIVE — the lane connectivity this router graph is built on.
• Detecting Dangling Lanes and Connectivity Gaps — the validation that catches the islands this reachability gate flags.
• Validating Turn Restrictions in Road Graphs — how turn edges are constrained by regulatory rules.

Up one level: Lane Geometry Extraction & Road Network Processing — the parent domain whose routing layer this construction produces.