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"

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.

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.

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.