Pathsolvers

Generic A*

astar_search is the low-level A* implementation shared by GraphPlanner and grid_astar. It works on any hashable node type — you can use it directly to build a custom planner. See Using astar_search directly for a worked example.

map_data.pathsolver.astar.astar_search(start_node, goal_node, get_neighbors_func, heuristic_func)

Search a graph using the A* algorithm.

Parameters:

Name	Type	Description	Default
start_node	object	The starting node.	required
goal_node	object	The goal node.	required
get_neighbors_func	callable	A function that takes a node and returns an iterable of (neighbor, cost) tuples.	required
heuristic_func	callable	A function that takes a node and returns the estimated cost to the goal.	required

Returns:

Name	Type	Description
path	list or None	The path from start to goal as a list of nodes, or None if no path exists.

Source code in map_data/pathsolver/astar.py

def astar_search[N](
    start_node: N,
    goal_node: N,
    get_neighbors_func: Callable[[N], Iterable[tuple[N, float]]],
    heuristic_func: Callable[[N], float],
) -> list[N] | None:
    """
    Search a graph using the A* algorithm.

    Parameters
    ----------
    start_node : object
        The starting node.
    goal_node : object
        The goal node.
    get_neighbors_func : callable
        A function that takes a node and returns an iterable of (neighbor, cost) tuples.
    heuristic_func : callable
        A function that takes a node and returns the estimated cost to the goal.

    Returns
    -------
    path : list or None
        The path from start to goal as a list of nodes, or None if no path exists.

    """
    count = 0
    # Priority queue stores (f_score, count, current_node)
    # f_score = cost + heuristic  # noqa: ERA001
    q: list[tuple[float, int, N]] = [(0, count, start_node)]

    # visited maps node -> (cost, parent)
    visited: dict[N, tuple[float, N | None]] = {start_node: (0, None)}
    closed = set()

    while q:
        (_, _, u) = heapq.heappop(q)
        if u in closed:
            continue
        closed.add(u)
        cost = visited[u][0]

        if u == goal_node:
            # Path found, reconstruct
            path = []
            curr: N | None = u
            while curr is not None:
                path.append(curr)
                curr = visited[curr][1]
            return path[::-1]

        for v, dist in get_neighbors_func(u):
            new_cost = cost + dist
            if v not in visited or new_cost < visited[v][0]:
                visited[v] = (new_cost, u)
                h = heuristic_func(v)
                count += 1
                heapq.heappush(q, (new_cost + h, count, v))

    return None

Graph Planner

Graph-based path planner that routes along the OSM road and footway network.

Builds an undirected weighted graph from the ways stored in a :class:~map_data.map_data.MapData instance. Manually annotated paths (negative-ID ways) are spliced into the graph by projecting their endpoints onto the nearest OSM edge, so annotations extend the traversable network seamlessly.

Walkable areas (closed area=yes or multipolygon ways, e.g. pedestrian squares) can be crossed rather than only walked around: A* may hop between any two of an area's entries along the shortest path inside it (see :mod:map_data.pathsolver.walkable_area), and a waypoint inside an area stays where it is instead of snapping to its rim.

Planning is performed with A* (see :meth:plan). The planner operates entirely in UTM coordinates.

map_data.pathsolver.graph_planner.GraphPlanner.__init__(map_data, highway_types=None, max_snap_distance=DEFAULT_MAX_SNAP_DISTANCE, exclude_highway=NON_ROUTABLE_HIGHWAY_VALUES, traversability=None, highway_costs=None, surface_costs=None, path_cost_cap=None)

Initialize the graph planner.

Parameters:

Name	Type	Description	Default
map_data	MapData	Parsed map data containing the OSM ways and node cache.	required
highway_types	list of str	Way categories to include in the graph. Supported values are "footway" and "road". Defaults to ["footway"].	None
max_snap_distance	float	Maximum distance (metres) a waypoint passed to :meth:plan may be from the nearest graph edge to be snapped onto it. Waypoints farther than this fail the plan instead of snapping to an arbitrarily distant edge (default :data:DEFAULT_MAX_SNAP_DISTANCE).	DEFAULT_MAX_SNAP_DISTANCE
exclude_highway	iterable of str	highway tag values never routed over, whatever their category (default :data:~map_data.utils.way.NON_ROUTABLE_HIGHWAY_VALUES, i.e. stairs). A map loaded through :func:~map_data.annotations.load_mapdata_with_annotations has them removed already; this filter also covers callers that pass a raw :meth:MapData.load map.	NON_ROUTABLE_HIGHWAY_VALUES
traversability	TraversabilityRules or str or Path	Tag rules deciding which ways may be driven on at all and what extra cost they carry (None = the package's config/traversability.yaml, see :mod:map_data.traversability). exclude_highway is folded into them. Ways the rules reject are left out of the graph, again as a safety net for raw maps.	None
highway_costs	mapping	Cost per highway / surface tag value; None takes the tables from config/planner_defaults.yaml, the very ones the grid planner uses (:mod:map_data.pathsolver.way_cost). An edge of a way weighs length * (1 + way cost + rule cost), so a gravel detour is taken only when it is enough shorter; the reported route length stays geometric.	None
surface_costs	mapping	Cost per highway / surface tag value; None takes the tables from config/planner_defaults.yaml, the very ones the grid planner uses (:mod:map_data.pathsolver.way_cost). An edge of a way weighs length * (1 + way cost + rule cost), so a gravel detour is taken only when it is enough shorter; the reported route length stays geometric.	None
path_cost_cap	float	Upper bound of the highway + surface cost (None = the config value).	None

Source code in map_data/pathsolver/graph_planner.py

def __init__(
    self,
    map_data: "MapData",
    highway_types: list[str] | None = None,
    max_snap_distance: float = DEFAULT_MAX_SNAP_DISTANCE,
    exclude_highway: Iterable[str] = NON_ROUTABLE_HIGHWAY_VALUES,
    traversability: "TraversabilityRules | str | Path | None" = None,
    highway_costs: "Mapping[str, float] | None" = None,
    surface_costs: "Mapping[str, float] | None" = None,
    path_cost_cap: float | None = None,
) -> None:
    """
    Initialize the graph planner.

    Parameters
    ----------
    map_data : MapData
        Parsed map data containing the OSM ways and node cache.
    highway_types : list of str, optional
        Way categories to include in the graph. Supported values are
        ``"footway"`` and ``"road"``. Defaults to ``["footway"]``.
    max_snap_distance : float
        Maximum distance (metres) a waypoint passed to :meth:`plan` may
        be from the nearest graph edge to be snapped onto it. Waypoints
        farther than this fail the plan instead of snapping to an
        arbitrarily distant edge
        (default :data:`DEFAULT_MAX_SNAP_DISTANCE`).
    exclude_highway : iterable of str
        ``highway`` tag values never routed over, whatever their category
        (default :data:`~map_data.utils.way.NON_ROUTABLE_HIGHWAY_VALUES`,
        i.e. stairs). A map loaded through
        :func:`~map_data.annotations.load_mapdata_with_annotations` has
        them removed already; this filter also covers callers that pass a
        raw :meth:`MapData.load` map.
    traversability : TraversabilityRules or str or Path, optional
        Tag rules deciding which ways may be driven on at all and what
        extra cost they carry (``None`` = the package's
        ``config/traversability.yaml``, see
        :mod:`map_data.traversability`). ``exclude_highway`` is folded into
        them. Ways the rules reject are left out of the graph, again as a
        safety net for raw maps.
    highway_costs, surface_costs : mapping, optional
        Cost per ``highway`` / ``surface`` tag value; ``None`` takes the
        tables from ``config/planner_defaults.yaml``, the very ones the
        grid planner uses (:mod:`map_data.pathsolver.way_cost`). An edge of
        a way weighs ``length * (1 + way cost + rule cost)``, so a gravel
        detour is taken only when it is enough shorter; the *reported*
        route length stays geometric.
    path_cost_cap : float, optional
        Upper bound of the ``highway`` + ``surface`` cost (``None`` = the
        config value).

    """
    self.map_data = map_data
    self.highway_types = highway_types or ["footway"]
    self.max_snap_distance = max_snap_distance
    self.exclude_highway = frozenset(exclude_highway)
    self.traversability = load_traversability(traversability).extend(self.exclude_highway)
    self.highway_costs, self.surface_costs, self.path_cost_cap = load_cost_tables(
        highway_costs, surface_costs, path_cost_cap
    )
    self.nodes: dict[int, np.ndarray] = self.map_data.get_points()
    self.graph: dict[int, list[tuple[int, float]]] = {}
    self._build_graph()

map_data.pathsolver.graph_planner.GraphPlanner.plan(path_utm, keep_start=False, keep_goal=False)

Plan a path through a sequence of UTM waypoints along the graph.

Each consecutive pair of waypoints is routed independently. The waypoints are snapped to the nearest graph edge before planning, so they do not need to lie exactly on the network; the returned route consists of on-network points only, the requested waypoints being represented by their projections rather than repeated verbatim (see :func:_drop_stacked_points).

Parameters:

Name	Type	Description	Default
path_utm	ndarray	Array of shape (N, 2) containing [x, y] UTM coordinates of the desired waypoints, in order. At least two waypoints are required.	required
keep_start	bool	Prepend the first waypoint verbatim instead of starting the route at its projection. For planning from the robot's own pose, where the route has to begin where the robot actually is; the leading off-network leg is then the robot's way onto the network. Only the first waypoint is treated this way — doing it for a waypoint in the middle of the route would produce a spur out to it and back.	False
keep_goal	bool	Append the last waypoint verbatim after its projection, so the route ends at the requested coordinate rather than :attr:max_snap_distance metres short of it. The final, off-network leg is skipped when the projection is already within :data:MIN_GOAL_LEG_LENGTH of the goal.	False

Returns:

Type	Description
ndarray or None	Concatenated path as an (M, 2) UTM coordinate array with coincident vertices and sub-metre spurs removed, or None if fewer than two waypoints were given, a waypoint is farther than :attr:max_snap_distance from the network, or any segment could not be routed.

Source code in map_data/pathsolver/graph_planner.py

def plan(
    self,
    path_utm: np.ndarray,
    keep_start: bool = False,
    keep_goal: bool = False,
) -> np.ndarray | None:
    """
    Plan a path through a sequence of UTM waypoints along the graph.

    Each consecutive pair of waypoints is routed independently. The
    waypoints are snapped to the nearest graph edge before planning,
    so they do not need to lie exactly on the network; the returned
    route consists of on-network points only, the requested waypoints
    being represented by their projections rather than repeated
    verbatim (see :func:`_drop_stacked_points`).

    Parameters
    ----------
    path_utm : np.ndarray
        Array of shape ``(N, 2)`` containing ``[x, y]`` UTM coordinates
        of the desired waypoints, in order. At least two waypoints are
        required.
    keep_start : bool
        Prepend the first waypoint verbatim instead of starting the route
        at its projection. For planning from the robot's own pose, where
        the route has to begin where the robot actually is; the leading
        off-network leg is then the robot's way onto the network. Only the
        first waypoint is treated this way — doing it for a waypoint in
        the middle of the route would produce a spur out to it and back.
    keep_goal : bool
        Append the last waypoint verbatim after its projection, so the
        route ends at the requested coordinate rather than
        :attr:`max_snap_distance` metres short of it. The final,
        off-network leg is skipped when the projection is already within
        :data:`MIN_GOAL_LEG_LENGTH` of the goal.

    Returns
    -------
    np.ndarray or None
        Concatenated path as an ``(M, 2)`` UTM coordinate array with
        coincident vertices and sub-metre spurs removed, or
        ``None`` if fewer than two waypoints were given, a waypoint is
        farther than :attr:`max_snap_distance` from the network, or any
        segment could not be routed.

    """
    if len(path_utm) < 2:
        logger.warning(
            "plan() requires at least two waypoints, got %d; cannot plan.",
            len(path_utm),
        )
        return None

    full_path: list[np.ndarray] = []

    for i in range(len(path_utm) - 1):
        id_s = "temp_start"
        id_g = "temp_goal"
        ends = {
            id_s: np.asarray(path_utm[i], dtype=float)[:2],
            id_g: np.asarray(path_utm[i + 1], dtype=float)[:2],
        }

        # Positions, adjacency and crossing geometry of the two temporary
        # nodes, kept in separate dicts (rather than one mixing them under
        # string/int keys) so none needs a cast to satisfy the type checker.
        positions: dict[int | str, np.ndarray] = {}
        extra_adj: dict[int | str, list[tuple[int | str, float]]] = {}
        extra_via: dict[tuple[int | str, int | str], list[np.ndarray]] = {}
        snapped: dict[str, tuple[int, int, float]] = {}

        for tid, waypoint in ends.items():
            if self._area_at(waypoint) is not None:
                # Inside a walkable area the waypoint itself is the node.
                positions[tid] = waypoint
                continue

            edge_info, dist = self._find_closest_edge(waypoint)
            if edge_info is None:
                return None
            if dist > self.max_snap_distance:
                logger.warning(
                    "Waypoint (%.1f, %.1f) is %.1f m from the nearest graph "
                    "edge, beyond the %.1f m snap limit; cannot plan.",
                    waypoint[0],
                    waypoint[1],
                    dist,
                    self.max_snap_distance,
                )
                return None
            n1, n2, proj, factor = edge_info
            positions[tid] = proj
            snapped[tid] = (n1, n2, factor)
            for node in (n1, n2):
                # Distance from the projection to one end of its edge, priced like the way.
                end = self.nodes[node].ravel()[:2]
                cost = math.hypot(proj[0] - end[0], proj[1] - end[1]) * factor
                _link(extra_adj, extra_via, tid, node, cost, None)

        # A node inside a walkable area, or snapped onto or next to one (its
        # rim, say, whose own nodes need not be entries), is joined to the
        # area's entries, and to the other node when both are in it, by
        # in-area crossings.
        areas = {tid: self._area_at(p, ENTRY_TOLERANCE) for tid, p in positions.items()}
        for tid in (id_s, id_g):
            area = areas[tid]
            if area is None:
                continue
            shared = tid == id_s and areas[id_g] is area
            goal = positions[id_g] if shared else None
            to_entries, to_goal = area.from_point(positions[tid], goal)
            for node, (cost, points) in to_entries.items():
                _link(extra_adj, extra_via, tid, node, cost, points)
            if to_goal is not None:
                _link(extra_adj, extra_via, id_s, id_g, *to_goal)

        # Special case: start and goal on the same edge
        if id_s in snapped and id_g in snapped:
            n_s1, n_s2, f_s = snapped[id_s]
            if {n_s1, n_s2} == set(snapped[id_g][:2]):
                p_s, p_g = positions[id_s], positions[id_g]
                cost = math.hypot(p_s[0] - p_g[0], p_s[1] - p_g[1]) * f_s
                _link(extra_adj, extra_via, id_s, id_g, cost, None)

        # Route between temporary nodes
        segment = self._route_segment(id_s, id_g, positions, extra_adj, extra_via)
        if segment is None:
            return None

        # segment is [p_proj_s, ..., p_proj_g] — entirely on the network. The
        # clicked waypoints themselves are deliberately left out: re-inserting
        # an off-network click between its own projections turns every via
        # point into a degenerate out-and-back spur of stacked points.
        full_path.extend(segment)

    if keep_start:
        full_path.insert(0, np.asarray(path_utm[0], dtype=float)[:2])
    if keep_goal:
        goal = np.asarray(path_utm[-1], dtype=float)[:2]
        if not full_path or float(np.linalg.norm(goal - full_path[-1])) > MIN_GOAL_LEG_LENGTH:
            full_path.append(goal)

    return _drop_stacked_points(full_path)

map_data.pathsolver.graph_planner.GraphPlanner._route_segment(id_s, id_g, positions, extra_adj, extra_via)

Run A from id_s to id_g* over the graph plus a local subgraph.

positions gives the world coordinate of the two temporary snapped nodes (everything else resolves through :attr:nodes); extra_adj gives their (and their edge endpoints') extra adjacency, and extra_via the geometry of those extra hops that cross an area. Kept as plain dicts rather than one mixing them under string/int keys, so none needs a cast to satisfy the type checker. Besides the graph's own edges, an entry of a walkable area neighbours the area's other entries.

Source code in map_data/pathsolver/graph_planner.py

def _route_segment(
    self,
    id_s: int | str,
    id_g: int | str,
    positions: dict[int | str, np.ndarray],
    extra_adj: dict[int | str, list[tuple[int | str, float]]],
    extra_via: "dict[tuple[int | str, int | str], list[np.ndarray]]",
) -> list[np.ndarray] | None:
    """
    Run A* from *id_s* to *id_g* over the graph plus a local subgraph.

    *positions* gives the world coordinate of the two temporary snapped
    nodes (everything else resolves through :attr:`nodes`); *extra_adj*
    gives their (and their edge endpoints') extra adjacency, and
    *extra_via* the geometry of those extra hops that cross an area. Kept
    as plain dicts rather than one mixing them under string/int keys, so
    none needs a cast to satisfy the type checker. Besides the graph's own
    edges, an entry of a walkable area neighbours the area's other entries.
    """

    def get_pos(node: int | str) -> np.ndarray:
        pos = positions.get(node)
        return pos if pos is not None else self.nodes[node].ravel()[:2]  # type: ignore[index]

    def get_neighbors(u: int | str) -> list[tuple[int | str, float]]:
        neighs: list[tuple[int | str, float]] = []
        if isinstance(u, int):
            neighs.extend(self.graph.get(u, []))
            for area in self._node_areas.get(u, ()):
                neighs.extend((v, cost) for v, (cost, _) in area.crossings(u).items())
        neighs.extend(extra_adj.get(u, []))
        return neighs

    goal_x, goal_y = get_pos(id_g)

    def heuristic(u: int | str) -> float:
        pos = get_pos(u)
        return math.hypot(pos[0] - goal_x, pos[1] - goal_y)

    node_path = astar_search(id_s, id_g, get_neighbors, heuristic)
    if node_path is None:
        return None
    route = [get_pos(node_path[0])]
    for a, b in itertools.pairwise(node_path):
        via = extra_via.get((a, b)) or self._crossing_points(a, b)
        route.extend(via[1:] if via else [get_pos(b)])
    return route

Walkable areas

Open walkable areas (pedestrian squares) for the graph planner.

A pedestrian square is mapped as a closed area=yes way, i.e. by its outline only. Routed as a chain of nodes like any other way, it makes a route walk the rim of the square instead of crossing it. :class:WalkableArea keeps the polygon and lets the planner cross it: between two of its entries (network nodes on, inside or within :data:ENTRY_TOLERANCE of the area) it prices the shortest path that stays inside the polygon, holes included.

Such a path bends only at reflex corners of the outline and at hole vertices, so a visibility graph over the entries and those corners is enough. It is built on first use and kept with the area; the planner's global adjacency gains no edges.

WalkableArea

Shortest in-area crossings between the entries of one walkable polygon.

Costs are the in-polygon path length times factor, the weight multiplier of the area's own edges, so a crossing is priced like walking its rim and the planner's straight-line A* heuristic stays admissible.

__init__(polygon, factor, entries)

Parameters:

Name	Type	Description	Default
polygon	Polygon	The area in UTM coordinates, holes included.	required
factor	float	Weight multiplier of the area's edges (at least 1).	required
entries	dict	Node id -> [x, y] of the network nodes that enter the area. A node just outside is joined to its nearest point on the boundary.	required

covers(point)

Return True if point lies in the area (boundary included, holes excluded).

crossings(node)

Crossings from entry node to every other entry it can reach inside the area.

from_point(point, goal=None)

Crossings from a point in or next to the area to its entries and, when given, to goal.

A point (or goal) just outside joins the area at its nearest boundary point, like an entry does. The goal crossing is None when goal is not given or cannot be reached from point inside the area.

Grid A*

map_data.pathsolver.grid_astar.grid_astar(grid, start_utm, goal_utm, low, cs, *, simplify_path=True, grid_cost_weight=GRID_COST_WEIGHT)

Optimized A* search on a 2D grid.

Parameters:

Name	Type	Description	Default
grid	ndarray	2D grid of costs (Y, X). inf means blocked.	required
start_utm	tuple or ndarray	Starting point in UTM coordinates.	required
goal_utm	tuple or ndarray	Goal point in UTM coordinates.	required
low	tuple	(min_x, min_y) of the grid in UTM metres.	required
cs	float	Cell size of the grid in metres.	required
simplify_path	bool	Whether to simplify the resulting path with Douglas-Peucker.	True

Returns:

Type	Description
ndarray or None	Found path as UTM coordinate array, or None if no path exists.

Source code in map_data/pathsolver/grid_astar.py

def grid_astar(
    grid: np.ndarray,
    start_utm: tuple[float, float] | np.ndarray,
    goal_utm: tuple[float, float] | np.ndarray,
    low: tuple[float, float],
    cs: float,
    *,
    simplify_path: bool = True,
    grid_cost_weight: float = GRID_COST_WEIGHT,
) -> np.ndarray | None:
    """
    Optimized A* search on a 2D grid.

    Parameters
    ----------
    grid : np.ndarray
        2D grid of costs (Y, X). ``inf`` means blocked.
    start_utm : tuple or np.ndarray
        Starting point in UTM coordinates.
    goal_utm : tuple or np.ndarray
        Goal point in UTM coordinates.
    low : tuple
        ``(min_x, min_y)`` of the grid in UTM metres.
    cs : float
        Cell size of the grid in metres.
    simplify_path : bool
        Whether to simplify the resulting path with Douglas-Peucker.

    Returns
    -------
    np.ndarray or None
        Found path as UTM coordinate array, or ``None`` if no path exists.

    """
    ny, nx = grid.shape

    # Convert UTM to grid indices
    def to_idx(p: tuple[float, float] | np.ndarray) -> tuple[int, int]:
        ix = int(np.floor((p[0] - low[0]) / cs))
        iy = int(np.floor((p[1] - low[1]) / cs))
        return ix, iy

    start_ix, start_iy = to_idx(start_utm)
    goal_ix, goal_iy = to_idx(goal_utm)

    if not (0 <= goal_ix < nx and 0 <= goal_iy < ny):
        logger.warning("Goal %s is outside the grid bounds; cannot plan path.", goal_utm)
        return None
    if not (0 <= start_ix < nx and 0 <= start_iy < ny):
        logger.warning("Start %s is outside the grid bounds; cannot plan path.", start_utm)
        return None

    if start_ix == goal_ix and start_iy == goal_iy:
        return np.array([start_utm, goal_utm])

    # Pre-calculate costs and pad with infinity to avoid boundary checks
    # grid is assumed to be 0.0 near paths, 1.0 away from paths.
    # Base traversal cost is 1.0 + grid_value * grid_cost_weight
    costs = 1.0 + grid * grid_cost_weight
    padded_costs = np.full((ny + 2, nx + 2), np.inf, dtype=np.float32)
    padded_costs[1:-1, 1:-1] = costs

    # Flattened grid size with padding
    p_nx = nx + 2
    p_ny = ny + 2

    # Plain Python floats: numpy scalar arithmetic dominates this inner loop.
    # The costs are float32, and every g score is rounded back to float32 through
    # _f32 so the expansion order (and the path) stays bit-identical to the
    # previous all-numpy version.
    _f32 = array.array("f", [0.0])
    g_scores = array.array("f", [math.inf]) * (p_ny * p_nx)
    parents = np.full(p_ny * p_nx, -1, dtype=np.int32)

    start_flat = (start_iy + 1) * p_nx + (start_ix + 1)
    goal_flat = (goal_iy + 1) * p_nx + (goal_ix + 1)
    g_scores[start_flat] = 0.0

    # Priority queue: (f_score, g_score, ix, iy)
    h0 = math.sqrt((start_ix - goal_ix) ** 2 + (start_iy - goal_iy) ** 2)
    pq = [(h0, 0.0, start_ix, start_iy)]

    # Neighbor offsets in flattened padded grid (dy * p_nx + dx, dist, ortho_offsets).
    # For diagonal moves, ortho_offsets are the two edge-adjacent cells the move
    # passes between; both must be traversable so the path cannot cut through
    # the corner where two blocked cells touch diagonally.
    neighbors_data = []
    for dy in [-1, 0, 1]:
        for dx in [-1, 0, 1]:
            if dx == 0 and dy == 0:
                continue
            ortho_offsets = (dy * p_nx, dx) if dx != 0 and dy != 0 else ()
            dist = float(np.float32(math.sqrt(dx**2 + dy**2)))
            neighbors_data.append((dy * p_nx + dx, dist, ortho_offsets))

    flat_costs = padded_costs.ravel().tolist()

    while pq:
        _f, g_pushed, ix, iy = heapq.heappop(pq)

        u_flat = (iy + 1) * p_nx + (ix + 1)
        if g_scores[u_flat] < g_pushed - 1e-4:
            continue

        if u_flat == goal_flat:
            # Path found, reconstruct
            path_indices = []
            curr = u_flat
            while curr != -1:
                c_iy, c_ix = divmod(curr, p_nx)
                path_indices.append((c_ix - 1, c_iy - 1))
                curr = parents[curr]
            path_indices.reverse()

            # Convert back to UTM (cell centers, matching the grid's
            # cell-center sampling convention)
            path = np.array(
                [[(ix + 0.5) * cs + low[0], (iy + 0.5) * cs + low[1]] for ix, iy in path_indices],
            )

            # Simplify path, collision-checking every shortcut against the grid
            if simplify_path and len(path) > 2:
                path = simplify_path_checked(
                    path,
                    cs / 2.0,
                    lambda p1, p2: grid_segment_blocked(grid, p1, p2, low, cs),
                )
            return path

        current_g = g_scores[u_flat]

        for offset, dist, ortho_offsets in neighbors_data:
            v_flat = u_flat + offset
            cost_val = flat_costs[v_flat]

            if cost_val == math.inf:
                continue

            # Diagonal moves must not slip between two corner-touching
            # blocked cells: both edge-adjacent cells have to be free.
            if ortho_offsets and any(flat_costs[u_flat + o] == math.inf for o in ortho_offsets):
                continue

            _f32[0] = dist * cost_val
            _f32[0] = current_g + _f32[0]
            new_g = _f32[0]
            if new_g < g_scores[v_flat]:
                g_scores[v_flat] = new_g
                parents[v_flat] = u_flat
                v_iy, v_ix = divmod(v_flat, p_nx)
                h = math.sqrt((v_ix - 1 - goal_ix) ** 2 + (v_iy - 1 - goal_iy) ** 2)
                heapq.heappush(pq, (new_g + h, new_g, v_ix - 1, v_iy - 1))

    return None

RRT*

Rapidly-exploring Random Tree Star (RRT*) path planner.

Builds a collision-free tree by randomly sampling the free space and rewiring edges to minimise path cost. The planner is asymptotically optimal: given enough iterations it converges to the shortest feasible path.

Collision checking uses two sources simultaneously:

• A Shapely STRtree of barrier polygons (hard obstacles).
• A 2-D cost grid where cells at or above traversability_threshold are treated as blocked.

Traversable cells contribute a weighted cost to the edge cost, so the planner naturally prefers low-cost corridors (e.g. footways) over open terrain.

map_data.pathsolver.rrt_star.RRTStar.__init__(start, goal, obstacles, obstacles_tree, grid, low, grid_scale=1.0, max_iter=2000, step_size=2.0, neighbor_radius=5.0, traversability_threshold=10.0, grid_cost_weight=GRID_COST_WEIGHT, *, transfer_id=None, improve_after_goal=False, improve_iter=200, informed=True, adaptive_radius=True, rng=None)

Initialize the RRT* planner.

Parameters:

Name	Type	Description	Default
start	ndarray	Starting position as a 2-element array [x, y] in world (UTM) coordinates.	required
goal	ndarray	Goal position as a 2-element array [x, y] in world coordinates.	required
obstacles	list	Shapely geometries representing hard barriers. Not read directly (collision checks go through obstacles_tree); accepted so callers can pass the same pair they already have.	required
obstacles_tree	STRtree or None	Pre-built Shapely STRtree index over obstacles. Pass None to skip polygon-based collision checking (grid only).	required
grid	ndarray	2-D cost array with shape (Y, X). A value of 0.0 means fully free; values at or above traversability_threshold are blocked. Intermediate values increase edge cost.	required
low	tuple of float	(min_x, min_y) corner of the grid in world coordinates.	required
grid_scale	float	Metres per grid cell (default 1.0).	1.0
max_iter	int	Maximum number of RRT* iterations (default 2000).	2000
step_size	float	Maximum distance the tree extends toward a sampled point per iteration in metres (default 2.0).	2.0
neighbor_radius	float	Radius in metres within which nearby nodes are considered for rewiring (default 5.0).	5.0
traversability_threshold	float	Grid cost at which a cell is considered an obstacle (default 10.0). np.inf marks cells as hard obstacles.	10.0
transfer_id	str or None	Optional identifier used to check for external cancellation signals during planning. Pass None to disable.	None
improve_after_goal	bool	If True, continue iterating after the goal is first reached to find a lower-cost path via informed sampling and rewiring. If False (default), return as soon as the goal is reached.	False
improve_iter	int	With improve_after_goal, the most extra iterations spent improving once the goal is first reached (default 200); max_iter still caps the total.	200
informed	bool	If True (default), sample from the informed ellipse once a solution exists (see :meth:_sample_informed), shrinking as improve_after_goal lowers the best cost found.	True
adaptive_radius	bool	If True (default), shrink the rewiring radius as the tree grows per the RRT asymptotic-optimality formula, instead of using a fixed neighbor_radius*.	True
rng	Random or None	Random source for every sampling draw. Defaults to a fresh :class:random.Random per planner, so instances never share the module-global stream and concurrent planners cannot interleave each other's draws. Pass random.Random(seed) to make a run reproducible.	None

Source code in map_data/pathsolver/rrt_star.py

def __init__(
    self,
    start: np.ndarray,
    goal: np.ndarray,
    obstacles: list[sh.geometry.base.BaseGeometry],
    obstacles_tree: STRtree | None,
    grid: np.ndarray,
    low: tuple[float, float],
    grid_scale: float = 1.0,
    max_iter: int = 2000,
    step_size: float = 2.0,
    neighbor_radius: float = 5.0,
    traversability_threshold: float = 10.0,  # inf is blocked, high values are expensive
    grid_cost_weight: float = GRID_COST_WEIGHT,
    *,
    transfer_id: str | None = None,
    improve_after_goal: bool = False,
    improve_iter: int = 200,
    informed: bool = True,
    adaptive_radius: bool = True,
    rng: random.Random | None = None,
) -> None:
    """
    Initialize the RRT* planner.

    Parameters
    ----------
    start : np.ndarray
        Starting position as a 2-element array ``[x, y]`` in world
        (UTM) coordinates.
    goal : np.ndarray
        Goal position as a 2-element array ``[x, y]`` in world
        coordinates.
    obstacles : list
        Shapely geometries representing hard barriers. Not read directly
        (collision checks go through *obstacles_tree*); accepted so
        callers can pass the same pair they already have.
    obstacles_tree : STRtree or None
        Pre-built Shapely STRtree index over *obstacles*. Pass ``None``
        to skip polygon-based collision checking (grid only).
    grid : np.ndarray
        2-D cost array with shape ``(Y, X)``. A value of ``0.0`` means
        fully free; values at or above *traversability_threshold* are
        blocked. Intermediate values increase edge cost.
    low : tuple of float
        ``(min_x, min_y)`` corner of the grid in world coordinates.
    grid_scale : float
        Metres per grid cell (default ``1.0``).
    max_iter : int
        Maximum number of RRT* iterations (default ``2000``).
    step_size : float
        Maximum distance the tree extends toward a sampled point per
        iteration in metres (default ``2.0``).
    neighbor_radius : float
        Radius in metres within which nearby nodes are considered for
        rewiring (default ``5.0``).
    traversability_threshold : float
        Grid cost at which a cell is considered an obstacle
        (default ``10.0``). ``np.inf`` marks cells as hard obstacles.
    transfer_id : str or None
        Optional identifier used to check for external cancellation
        signals during planning. Pass ``None`` to disable.
    improve_after_goal : bool
        If ``True``, continue iterating after the goal is first reached
        to find a lower-cost path via informed sampling and rewiring. If
        ``False`` (default), return as soon as the goal is reached.
    improve_iter : int
        With *improve_after_goal*, the most extra iterations spent
        improving once the goal is first reached (default ``200``);
        *max_iter* still caps the total.
    informed : bool
        If ``True`` (default), sample from the informed ellipse once a
        solution exists (see :meth:`_sample_informed`), shrinking as
        *improve_after_goal* lowers the best cost found.
    adaptive_radius : bool
        If ``True`` (default), shrink the rewiring radius as the tree
        grows per the RRT* asymptotic-optimality formula, instead of
        using a fixed *neighbor_radius*.
    rng : random.Random or None
        Random source for every sampling draw. Defaults to a fresh
        :class:`random.Random` per planner, so instances never share the
        module-global stream and concurrent planners cannot interleave
        each other's draws. Pass ``random.Random(seed)`` to make a run
        reproducible.

    """
    self.start = start
    self.goal = goal
    # obstacles itself is not read (collision checks go through
    # obstacles_tree); kept as a parameter so callers can pass the same
    # (geometries, tree) pair they already have.
    self.obstacles_tree = obstacles_tree
    self.grid = grid  # (Y, X)
    self.grid_shape = grid.shape
    self.low = np.array(low)
    self.grid_scale = grid_scale
    self.max_iter = max_iter
    self.step_size = step_size
    self.neighbor_radius = neighbor_radius
    self.nodes = [self.start]
    self.parent: dict[int, int | None] = {0: None}
    self.cost = {0: 0.0}
    # Children index (inverse of `parent`), needed to propagate cost
    # changes to descendants when a node is rewired.
    self._children: dict[int, set[int]] = {0: set()}

    self._nodes_buf = np.empty((max_iter + 2, 2), dtype=np.float64)
    self._nodes_buf[0] = self.start
    self.goal_tolerance = step_size
    self.traversability_threshold = traversability_threshold
    self.grid_cost_weight = grid_cost_weight
    self.transfer_id = transfer_id
    self.improve_after_goal = improve_after_goal
    self.improve_iter = improve_iter
    self.informed = informed
    self.adaptive_radius = adaptive_radius
    # Own random stream: a fresh Random() keeps each planner independent of
    # the module-global one (and of other planners running in parallel
    # request threads), while an injected one makes the run reproducible.
    self._rng = rng if rng is not None else random.Random()
    self._best_cost: float = float("inf")
    self._kdtree: cKDTree | None = None
    self._kdtree_n: int = 0

    # Limit sampling area
    dist = np.linalg.norm(self.goal - self.start)
    margin = max(dist * 0.5, step_size * 10)
    self._sample_min = np.minimum(self.start, self.goal) - margin
    self._sample_max = np.maximum(self.start, self.goal) + margin

    # Clip to grid
    grid_max_x = self.low[0] + self.grid_shape[1] * grid_scale
    grid_max_y = self.low[1] + self.grid_shape[0] * grid_scale
    self._sample_min = np.maximum(self._sample_min, self.low)
    self._sample_max = np.minimum(self._sample_max, [grid_max_x, grid_max_y])

    # Informed RRT*: precompute ellipse geometry
    d = self.goal - self.start
    self._c_min: float = float(np.linalg.norm(d))
    self._ellipse_center: np.ndarray = (self.start + self.goal) / 2.0
    theta = math.atan2(float(d[1]), float(d[0]))
    ct, st = math.cos(theta), math.sin(theta)
    self._C_be: np.ndarray = np.array([[ct, -st], [st, ct]])

    # Adaptive radius: gamma* for 2-D from the asymptotic optimality formula
    sample_area = float(np.prod(self._sample_max - self._sample_min))
    self._gamma: float = 2.449 * math.sqrt(max(sample_area, 1.0) / math.pi)

    # Precompute traversable cells for faster sampling
    _xi_lo = max(0, int((self._sample_min[0] - self.low[0]) / grid_scale))
    _xi_hi = min(
        self.grid_shape[1],
        int(np.ceil((self._sample_max[0] - self.low[0]) / grid_scale)),
    )
    _yi_lo = max(0, int((self._sample_min[1] - self.low[1]) / grid_scale))
    _yi_hi = min(
        self.grid_shape[0],
        int(np.ceil((self._sample_max[1] - self.low[1]) / grid_scale)),
    )

    _sub = self.grid[_yi_lo:_yi_hi, _xi_lo:_xi_hi]
    _ys, _xs = np.where(_sub < self.traversability_threshold)
    if len(_xs) > 0:
        self._trav_xs = (_xs + _xi_lo) * grid_scale + self.low[0]
        self._trav_ys = (_ys + _yi_lo) * grid_scale + self.low[1]
    else:
        self._trav_xs = None
        self._trav_ys = None

map_data.pathsolver.rrt_star.RRTStar.find_path()

Run the RRT* algorithm and return the planned path.

Iterates up to max_iter times, growing the tree from start toward randomly sampled points and rewiring edges to reduce cost. The goal is sampled directly 10 % of the time to encourage convergence.

Returns:

Type	Description
ndarray or None	Path as an (N, 2) array of [x, y] world coordinates, or None if no collision-free path was found within the iteration budget or if planning was cancelled via transfer_id.

Source code in map_data/pathsolver/rrt_star.py

def find_path(self) -> np.ndarray | None:
    """
    Run the RRT* algorithm and return the planned path.

    Iterates up to *max_iter* times, growing the tree from ``start``
    toward randomly sampled points and rewiring edges to reduce cost.
    The goal is sampled directly 10 % of the time to encourage
    convergence.

    Returns
    -------
    np.ndarray or None
        Path as an ``(N, 2)`` array of ``[x, y]`` world coordinates,
        or ``None`` if no collision-free path was found within the
        iteration budget or if planning was cancelled via *transfer_id*.

    """
    from .replan import _is_cancelled

    goal_idx = None
    goal_found_iter = None

    # ponytail: improvement is capped in iterations, not seconds; add a
    # wall-clock budget if per-iteration cost varies too much across maps.
    for i in range(self.max_iter):
        if _is_cancelled(self.transfer_id):
            return None
        if goal_found_iter is not None and i - goal_found_iter > self.improve_iter:
            break

        # Keep the informed-sampling ellipse in sync with the goal's true
        # cost: rewires (direct or propagated) may have improved it since
        # the goal-connection block last ran.
        if goal_idx is not None:
            self._best_cost = self.cost.get(goal_idx, float("inf"))

        rand_point = (
            self.goal if self._rng.random() < GOAL_SAMPLE_BIAS else self._sample_point()
        )
        nearest_idx = self._nearest_node(rand_point)
        new_point = self._steer(self.nodes[nearest_idx], rand_point)

        if self._point_blocked(new_point):
            continue

        collision, nearest_seg_cost = self._segment_cost(self.nodes[nearest_idx], new_point)
        if collision:
            continue

        new_idx = len(self.nodes)
        self.nodes.append(new_point)
        self._nodes_buf[new_idx] = new_point
        min_cost = self.cost[nearest_idx] + nearest_seg_cost
        min_parent = nearest_idx

        if self.adaptive_radius:
            n_eff = max(new_idx, int(math.e) + 1)
            r = min(self._gamma * math.sqrt(math.log(n_eff) / n_eff), self.neighbor_radius)
        else:
            r = self.neighbor_radius
        near_indices = self._get_near_nodes(new_point, r)
        for idx in near_indices:
            # Reuse already-computed cost for the nearest node
            if idx == nearest_idx:
                col, sc = False, nearest_seg_cost
            else:
                col, sc = self._segment_cost(self.nodes[idx], new_point)
            if not col:
                c = self.cost[idx] + sc
                if c < min_cost:
                    min_cost = c
                    min_parent = idx

        self._set_parent(new_idx, min_parent, min_cost)

        # Rewire
        for idx in near_indices:
            if idx == min_parent:
                continue
            col, sc = self._segment_cost(new_point, self.nodes[idx])
            if not col:
                new_c = self.cost[new_idx] + sc
                if new_c < self.cost[idx]:
                    self._set_parent(idx, new_idx, new_c)

        if np.linalg.norm(new_point - self.goal) < self.goal_tolerance:
            col, sc = self._segment_cost(new_point, self.goal)
            if not col:
                new_goal_cost = self.cost[new_idx] + sc
                if goal_idx is None:
                    goal_found_iter = i
                    goal_idx = len(self.nodes)
                    self.nodes.append(self.goal)
                    self._nodes_buf[goal_idx] = self.goal
                if new_goal_cost < self.cost.get(goal_idx, float("inf")):
                    self._set_parent(goal_idx, new_idx, new_goal_cost)
                    self._best_cost = new_goal_cost
                if not self.improve_after_goal:
                    path = self._reconstruct_path(goal_idx)
                    return np.array(path)

    if goal_idx is not None:
        # Final sync: rewires in the last iteration may have improved the
        # goal's cost after the loop-top sync last ran.
        self._best_cost = self.cost.get(goal_idx, float("inf"))
        path = self._reconstruct_path(goal_idx)
        return np.array(path)
    return None

map_data.pathsolver.rrt_star.RRTStar._sample_point()

Sample a random point.

When a solution exists and informed is enabled, samples uniformly from the informed ellipse. Otherwise biases toward traversable grid cells 90 % of the time.

Source code in map_data/pathsolver/rrt_star.py

def _sample_point(self) -> np.ndarray:
    """
    Sample a random point.

    When a solution exists and *informed* is enabled, samples uniformly
    from the informed ellipse.  Otherwise biases toward traversable grid
    cells 90 % of the time.
    """
    if self.informed and self._best_cost < float("inf"):
        return self._sample_informed()
    if self._trav_xs is not None and self._rng.random() > GOAL_SAMPLE_BIAS:
        idx = self._rng.randrange(len(self._trav_xs))
        return np.array(
            [
                self._trav_xs[idx]
                + self._rng.uniform(-self.grid_scale / 2, self.grid_scale / 2),
                self._trav_ys[idx]
                + self._rng.uniform(-self.grid_scale / 2, self.grid_scale / 2),
            ],
        )
    return np.array(
        [
            self._rng.uniform(self._sample_min[0], self._sample_max[0]),
            self._rng.uniform(self._sample_min[1], self._sample_max[1]),
        ],
    )

map_data.pathsolver.rrt_star.RRTStar._nearest_node(point)

Return the index of the tree node closest to point.

Uses a lazily rebuilt KD-tree for the bulk of the tree, plus a linear scan over nodes added since the last rebuild.

Source code in map_data/pathsolver/rrt_star.py

def _nearest_node(self, point: np.ndarray) -> int:
    """
    Return the index of the tree node closest to *point*.

    Uses a lazily rebuilt KD-tree for the bulk of the tree, plus a
    linear scan over nodes added since the last rebuild.
    """
    n = len(self.nodes)
    if self._kdtree is None or n - self._kdtree_n >= _KDTREE_REBUILD_INTERVAL:
        self._kdtree = cKDTree(self._nodes_buf[:n])
        self._kdtree_n = n

    _, best_idx = self._kdtree.query(point)
    best_d2 = float(((self._nodes_buf[best_idx] - point) ** 2).sum())

    # Linear scan over nodes added since the last rebuild
    for i in range(self._kdtree_n, n):
        d2 = float(((self._nodes_buf[i] - point) ** 2).sum())
        if d2 < best_d2:
            best_d2 = d2
            best_idx = i

    return int(best_idx)

map_data.pathsolver.rrt_star.RRTStar._steer(start, target)

Return a point at most step_size metres from start toward target.

Source code in map_data/pathsolver/rrt_star.py

def _steer(self, start: np.ndarray, target: np.ndarray) -> np.ndarray:
    """
    Return a point at most *step_size* metres from *start* toward *target*.
    """
    direction = target - start
    dist = np.linalg.norm(direction)
    if dist < self.step_size:
        return target
    return start + (direction / dist) * self.step_size

map_data.pathsolver.rrt_star.RRTStar._get_near_nodes(new_point, radius)

Return indices of all tree nodes within radius of new_point.

Source code in map_data/pathsolver/rrt_star.py

def _get_near_nodes(self, new_point: np.ndarray, radius: float) -> list[int]:
    """
    Return indices of all tree nodes within *radius* of *new_point*.
    """
    n = len(self.nodes)
    new_idx = n - 1  # node just appended by the caller
    r2 = radius**2

    # self._kdtree is never None here: the caller always runs
    # _nearest_node() first this iteration, which builds it.
    # KD-tree covers [0, _kdtree_n); new_point is never included in it.
    result: list[int] = list(self._kdtree.query_ball_point(new_point, radius))  # type: ignore[union-attr]

    # Linear scan over nodes added since the last rebuild, excluding new_point itself
    for i in range(self._kdtree_n, n):
        if i == new_idx:
            continue
        d2 = float(((self._nodes_buf[i] - new_point) ** 2).sum())
        if d2 < r2:
            result.append(i)

    return result

map_data.pathsolver.rrt_star.RRTStar._segment_cost(start, end)

Compute the cost of the segment from start to end.

Returns:

Type	Description
tuple of (bool, float)	(collision, cost) where collision is True if the segment intersects an obstacle or blocked grid cell, and cost is the weighted traversal cost dist * (1 + avg_grid_cost * 5). Returns (True, inf) on collision.

Source code in map_data/pathsolver/rrt_star.py

def _segment_cost(self, start: np.ndarray, end: np.ndarray) -> tuple[bool, float]:
    """
    Compute the cost of the segment from *start* to *end*.

    Returns
    -------
    tuple of (bool, float)
        ``(collision, cost)`` where *collision* is ``True`` if the
        segment intersects an obstacle or blocked grid cell, and *cost*
        is the weighted traversal cost ``dist * (1 + avg_grid_cost * 5)``.
        Returns ``(True, inf)`` on collision.

    """
    if (
        self.obstacles_tree
        and len(
            self.obstacles_tree.query(LineString([start, end]), predicate="intersects"),
        )
        > 0
    ):
        return True, float("inf")

    p1_grid = (
        int((start[0] - self.low[0]) / self.grid_scale),
        int((start[1] - self.low[1]) / self.grid_scale),
    )
    p2_grid = (
        int((end[0] - self.low[0]) / self.grid_scale),
        int((end[1] - self.low[1]) / self.grid_scale),
    )
    bres_line = _bresenham_cells(p1_grid, p2_grid)

    total_grid_cost = 0.0
    count = 0
    for x, y in bres_line:
        if 0 <= x < self.grid_shape[1] and 0 <= y < self.grid_shape[0]:
            c = self.grid[y, x]
            if c >= self.traversability_threshold:
                return True, float("inf")
            total_grid_cost += c
            count += 1

    avg_c = total_grid_cost / count if count > 0 else 0.0
    # Cost = dist * (1 + avg_grid_cost * penalty)  # noqa: ERA001
    # We use grid_cost_weight to match A* logic
    return False, float(np.linalg.norm(end - start) * (1.0 + avg_c * self.grid_cost_weight))

map_data.pathsolver.rrt_star.RRTStar._reconstruct_path(goal_idx)

Walk the parent chain from goal_idx back to the root and return the path.

Not simplified here: :meth:~map_data.pathsolver.replan.ReplanPath._post_process_path already runs a collision-checked Douglas-Peucker pass over the whole assembled route, so simplifying each RRT* segment first would just redo the same work at a smaller (and less effective) scale.

Source code in map_data/pathsolver/rrt_star.py

def _reconstruct_path(self, goal_idx: int) -> list[np.ndarray]:
    """
    Walk the parent chain from *goal_idx* back to the root and return the path.

    Not simplified here: :meth:`~map_data.pathsolver.replan.ReplanPath._post_process_path`
    already runs a collision-checked Douglas-Peucker pass over the whole
    assembled route, so simplifying each RRT* segment first would just
    redo the same work at a smaller (and less effective) scale.
    """
    path = []
    curr: int | None = goal_idx
    while curr is not None:
        path.append(self.nodes[curr])
        curr = self.parent[curr]
    path.reverse()
    return path

ReplanPath

ReplanPath is the local replanning engine used by the viewer's Planner mode and the replan CLI tool. It discretizes the area around the input waypoints into a cost grid, assigns traversal costs based on OSM highway and surface types, then finds a collision-free path using either Grid A or RRT.

Class attributes

These cost tables are loaded from config/planner_defaults.yaml at import time and can be overridden per-instance:

Attribute	Type	Description
HIGHWAY_COSTS	dict[str, float]	Per OSM highway value cost (0.0 = free, 1.0 = obstacle)
SURFACE_COSTS	dict[str, float]	Extra penalty per OSM surface value
DEFAULT_OFF_PATH_COST	float	Cost for cells not near any known way (default 0.9)
PATH_COST_CAP	float	Maximum cost a way cell can have (default 0.85, keeps ways preferred over off-path)

Constructor

ReplanPath(args, obstacles=None, transfer_id=None)

Parameter	Type	Description
args	argparse.Namespace	Planning parameters. Use parse_args([]) to get defaults.
obstacles	list[shapely.Geometry]	Obstacle geometries (from ways_to_shapely(md.barriers_list)).
transfer_id	str \| None	Optional UUID for cancellation via cancel_replan_backend().

args attributes used by ReplanPath:

Attribute	Default	Description
low	—	(min_x, min_y) lower bound of the planning area in UTM metres
high	—	(max_x, max_y) upper bound of the planning area in UTM metres
cell_size	0.25	Grid resolution in metres
inflate_obstacles	0.25	Buffer added to obstacle geometries in metres
simplify_path	True	Apply Douglas-Peucker simplification to the output
smooth_path	False	Apply gradient-descent smoothing after planning

Methods

fill_grid(map_data, highway_types=None, max_path_dist=2.0)

Populate the cost grid from map data. Must be called before replan().

Parameter	Default	Description
map_data	—	A loaded MapData object
highway_types	["footway"]	Which way categories to use. Pass ["footway", "road"] to include roads.
max_path_dist	2.0	Cells within this distance (m) of a known way receive an interpolated cost. Cells beyond receive DEFAULT_OFF_PATH_COST.

Cost model per cell: way_cost + (DEFAULT_OFF_PATH_COST - way_cost) × (dist / max_path_dist)², where way_cost = min(PATH_COST_CAP, HIGHWAY_COSTS[highway] + SURFACE_COSTS[surface]). Obstacle cells are set to inf.

replan(path, algorithm="astar")

Plan a path through the cost grid.

Parameter	Description
path	np.ndarray of shape (N, 2+) — input waypoints in UTM metres
algorithm	"astar" (Grid A) or "rrt" (RRT)

Returns np.ndarray (replanned path) or None (no path found, or the run was cancelled). Segments between consecutive waypoints are processed sequentially.

Cancellation

To cancel a running replan() call from another thread, pass a transfer_id UUID to the constructor and call:

from map_data.pathsolver.replan import cancel_replan_backend

cancel_replan_backend(transfer_id)

Minimal usage example

import numpy as np
from map_data.map_data import MapData
from map_data.pathsolver.replan import ReplanPath, parse_args
from map_data.utils.parsing import ways_to_shapely

md = MapData.load("coords.mapdata")

args = parse_args([])
args.low = (md.min_x, md.min_y)
args.high = (md.max_x, md.max_y)

replanner = ReplanPath(args, ways_to_shapely(md.barriers_list))
replanner.fill_grid(md, highway_types=["footway"], max_path_dist=2.0)

start = np.array([md.min_x + 10, md.min_y + 10])
goal = np.array([md.max_x - 10, md.max_y - 10])
new_path = replanner.replan(np.array([start, goal]), algorithm="astar")