Skip to content

Pathsolvers

Route planning

plan_route is the entry point the viewer, map_data_plan and route_planner share: graph or grid planning, request validation, waypoint spacing.

from map_data.annotations import load_mapdata_with_annotations
from map_data.pathsolver.route import RoutePlanningError, plan_route

md, _ = load_mapdata_with_annotations("my_area.mapdata")
try:
    result = plan_route(
        md, [(50.1050, 14.4220), (50.1072, 14.4280)], algorithm="graph", spacing=3.0
    )
except RoutePlanningError as e:
    print(e.reason, e.message)
else:
    print(f"{result.length_m:.0f} m, {len(result.latlon)} waypoints")

Plan a route through lat/lon waypoints on a :class:~map_data.map_data.MapData.

This is the headless core of the viewer's Planner screen (POST /api/create_replan): the same graph ("Paths only") and grid ("All terrain") planners, the same parameters, but callable from a script, the map_data_plan CLI or the route_planner ROS action server. The viewer route delegates to :func:plan_route so both stay in sync.

Failures are reported through :class:RoutePlanningError with a machine readable reason instead of a bare None:

too_few_points fewer than two waypoints (after adding the robot position); snap_too_far a waypoint is farther than max_snap_distance from every allowed way (graph planner); unreachable the waypoints lie on disconnected parts of the network (graph planner); no_path the grid planner found no path; start_blocked / goal_blocked a grid leg starts or ends inside an obstacle, with no free cell within :data:~map_data.pathsolver.replan.SNAP_RADIUS_M; cancelled the grid planner was cancelled through its transfer_id; start_outside_map / goal_outside_map the start or goal lies too far outside the map's area (wrong map file?); grid_too_large the requested grid would exceed max_grid_cells cells; route_too_long the route would have more than :data:MAX_ROUTE_POINTS vertices; bad_parameter an unknown algorithm / sub_algorithm, cell_size not finite and above zero, inflate_obstacles negative or not finite, or spacing not finite.

RouteResult dataclass

A planned route and how it relates to the requested waypoints.

RoutePlanningError

Bases: Exception

Planning failed; reason is one of the module-level reason strings.

plan_route(md, points_latlon, *, algorithm=GRAPH_ALGORITHM, sub_algorithm=None, highway_types=None, max_snap_distance=DEFAULT_MAX_SNAP_DISTANCE, spacing=0.0, cell_size=None, inflate_obstacles=None, simplify_path=None, smooth_path=None, grid_cost_weight=None, highway_costs=None, surface_costs=None, transfer_id=None, max_grid_cells=MAX_GRID_CELLS, planner=None, keep_start=False, keep_goal=False, exclude_highway=NON_ROUTABLE_HIGHWAY_VALUES, traversability=None, outside_map_tolerance=OUTSIDE_MAP_TOLERANCE_M)

Plan a route through points_latlon ([(lat, lon), ...], at least two).

algorithm="graph" routes along the allowed ways only (the viewer's Paths only); "astar"/"rrt" run the grid planner over the cost grid built from the map (All terrain). sub_algorithm picks the grid search ("astar"/"rrt"); None derives it from algorithm. Any other value fails with bad_parameter.

spacing > 0 resamples the result so waypoints are at most that far apart (a waypoint follower wants a few metres; below :data:MIN_SPACING_M fails with bad_parameter). changed and snap_distances are computed before resampling.

planner lets a caller reuse a prebuilt :class:GraphPlanner for md (building the graph costs ~0.3 s, planning on it ~1 ms); it must have been built for the same highway_types and max_snap_distance.

keep_start makes the graph route begin at the first waypoint verbatim rather than at its projection onto the network — for planning from the robot's own position, which is off the network by definition. It has no effect on the grid planners, which already start at the requested point.

keep_goal does the same at the other end: the route ends at the last requested coordinate instead of at its projection, so a goal off the mapped network is actually reached. The off-network leg is resampled by spacing like the rest of the route.

exclude_highway is the highway tag values the graph planner never routes over (stairs by default), and traversability the tag rules that decide the same question from the map's own tags (None = the package's config/traversability.yaml); both are ignored when planner is given, which brings its own.

highway_costs/surface_costs price the highway/surface tags for both planners: the grid planner's cells and the graph planner's edge weights (length * (1 + cost)). None takes config/planner_defaults.yaml, as does None for cell_size, inflate_obstacles, simplify_path, smooth_path and grid_cost_weight (see :func:~map_data.pathsolver.replan.grid_defaults).

A first or last point more than outside_map_tolerance metres outside the map's area fails with start_outside_map / goal_outside_map (see :func:check_in_map).

Raises :class:RoutePlanningError on failure.

goal_plan_kwargs(goal, defaults)

:func:plan_route keyword arguments for a PlanRoute action goal.

An empty or zero algorithm, highway_types, spacing, max_snap_distance, cell_size or inflate_obstacles takes the value of that name in defaults (the route_planner node's parameters); a negative spacing keeps the planner's own vertices. simplify_path and smooth_path are used as sent: a bool has no unset. A max_snap_distance above the default (or NaN) also takes the default.

check_in_map(md, point_utm, what, tolerance=OUTSIDE_MAP_TOLERANCE_M)

Raise <what>_outside_map (start / goal) when the point lies more than tolerance metres outside the map's area (the downloaded box, min_x..max_x / min_y..max_y). A start that far off almost always means the wrong map file is loaded; a goal, the wrong file or a code meant for another map.

Graph planner

A* planner over an undirected graph of a map's footways and/or roads, in UTM.

Annotation ways (negative ids) are spliced in by projecting their loose ends onto the nearest edge. Walkable areas (closed area=yes or multipolygon ways) can be crossed: A* may hop between any two of an area's entries (see :mod:map_data.pathsolver.walkable_area), and a waypoint inside an area is not snapped to its rim.

__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)

Build the graph from map_data's ways.

Parameters:

Name Type Description Default
highway_types list of str

"footway" and/or "road". Defaults to ["footway"].

 None
max_snap_distance float

A waypoint farther than this from every edge fails :meth:plan.

 DEFAULT_MAX_SNAP_DISTANCE
exclude_highway iterable of str

highway values never routed over (stairs by default). Covers raw :meth:MapData.load maps; annotated maps have them removed already.

 NON_ROUTABLE_HIGHWAY_VALUES
traversability TraversabilityRules or str or Path

Tag rules for which ways are drivable and their extra cost (None = config/traversability.yaml). exclude_highway is folded in.

 None
highway_costs mapping

Tag cost tables shared with the grid planner (None = config/planner_defaults.yaml). An edge weighs length * (1 + way cost + rule cost); reported lengths stay geometric.

 None
surface_costs mapping

Tag cost tables shared with the grid planner (None = config/planner_defaults.yaml). An edge weighs length * (1 + way cost + rule cost); reported lengths stay geometric.

 None
path_cost_cap float

Upper bound of the highway + surface cost (None = config).

 None

plan(path_utm, keep_start=False, keep_goal=False)

Route through (N, 2) UTM waypoints, each consecutive pair independently.

Waypoints are represented by their projections onto the network (or themselves inside a walkable area), not repeated verbatim.

Parameters:

Name Type Description Default
keep_start bool

Prepend the first waypoint verbatim, for planning from the robot's own pose. Only the first: a middle waypoint would become a spur.

 False
keep_goal bool

Append the last waypoint verbatim unless its projection is within :data:MIN_GOAL_LEG_LENGTH of it.

 False

Returns:

Type Description
ndarray or None

(M, 2) path without stacked points or short spurs, or None if fewer than two waypoints were given, a waypoint is beyond :attr:max_snap_distance, or a leg could not be routed.

snap_distance(point_utm)

Distance (m) from point_utm ([easting, northing]) to the nearest graph edge, 0 inside a walkable area, or inf when the graph has no edges. This is the distance :meth:plan compares against max_snap_distance.

edge_factor(way)

Weight multiplier of way's edges: 1 + way cost + rule cost.

The highway/surface cost comes from the shared tables (:func:~map_data.pathsolver.way_cost.way_cost, the grid planner's own prices), the extra from a cost: in the traversability rules. Never below 1, which keeps the straight-line A* heuristic admissible.

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.

Way costs

How expensive a way is to drive on, from its highway and surface tags.

Both planners charge the same price for the same way: the grid planner (:class:~map_data.pathsolver.grid_constructor.PathGrid) lowers a cell's cost towards :func:way_cost of the nearest way, and the graph planner (:class:~map_data.pathsolver.graph_planner.GraphPlanner) weighs an edge length * (1 + way_cost). The tables live in config/planner_defaults.yaml (highway_costs, surface_costs, path_cost_cap), so a route that prefers asphalt to gravel is a config change, not a code change.

Which ways are drivable at all is a separate question, answered from the tags by :mod:map_data.traversability; a rule there may add an extra cost on top of the tables for something they do not know about (informal=yes).

way_cost(tags, highway_costs, surface_costs, path_cost_cap=FALLBACK_PATH_COST_CAP)

Cost of driving on a way with these tags, in [0, path_cost_cap].

The highway cost and the surface cost add up and are capped: a gravel track is worse than a gravel footway, but nothing is worse than path_cost_cap, which keeps a mapped way cheaper than open terrain (default_off_path_cost).

Parameters:

Name Type Description Default
tags mapping or None

The way's OSM tags. A missing highway/surface is read as :data:DEFAULT_HIGHWAY / :data:DEFAULT_SURFACE.

 required
highway_costs mapping

The tables, e.g. from :func:load_cost_tables.

 required
surface_costs mapping

The tables, e.g. from :func:load_cost_tables.

 required
path_cost_cap float

Upper bound of the result.

 FALLBACK_PATH_COST_CAP

Returns:

Type Description
float

load_cost_tables(highway_costs=None, surface_costs=None, path_cost_cap=None)

Resolve the cost tables, filling in config/planner_defaults.yaml.

Parameters:

Name Type Description Default
highway_costs mapping

Explicit tables; None takes the ones from the config file (or the FALLBACK_* constants when it is missing).

 None
surface_costs mapping

Explicit tables; None takes the ones from the config file (or the FALLBACK_* constants when it is missing).

 None
path_cost_cap float

Highest cost any way may reach; None takes the config value.

 None

Returns:

Type Description
tuple

(highway_costs, surface_costs, path_cost_cap).

ReplanPath

Grid planning (Grid A or RRT) over a box around the waypoints. plan_route builds one per request; use it directly to plan on a grid of your own.

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

md = MapData.load("my_area.mapdata")
replanner = ReplanPath(
    ways_to_shapely(md.barriers_list), low=(md.min_x, md.min_y), high=(md.max_x, md.max_y)
)
replanner.fill_grid(md, highway_types=["footway"])

start = np.array([md.min_x + 10, md.min_y + 10])
goal = np.array([md.max_x - 10, md.max_y - 10])
path = replanner.replan(np.array([start, goal]), algorithm="astar")
if path is None:
    print(replanner.failure_reason)

Constructor keywords left None (cell_size, inflate_obstacles, simplify_path, smooth_path, grid_cost_weight, highway_costs, surface_costs) take the values of config/planner_defaults.yaml; see Planner configuration. The resolved tables are the instance attributes highway_costs, surface_costs, path_cost_cap and default_off_path_cost.

replan() returns the path, or None with failure_reason set to no_path, start_blocked, goal_blocked or cancelled. To cancel a run from another thread, construct it with a unique transfer_id and call cancel_replan_backend(transfer_id).

grid_defaults()

The grid planner's default settings: config/planner_defaults.yaml over :data:GRID_DEFAULTS, read on every call so file edits apply to the next plan.

The single source for :class:ReplanPath, :func:~map_data.pathsolver.route.plan_route, map_data_plan and the route_planner node's parameter defaults.

cancel_replan_backend(transfer_id)

Cancel the replan running, or about to run, under transfer_id.

Transfer ids must be unique per run: a cancel that lands before its run starts (while the map loads) still aborts it, so a reused id would abort the next run.

replan(path, algorithm='astar')

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

Populate the grid with costs based on map data.

max_path_dist None takes max_path_dist from planner_defaults.yaml.

fill_grid_lazily(map_data, highway_types=None, max_path_dist=None)

:meth:fill_grid, deferred until the first leg that hits an obstacle needs the grid: legs that don't are checked against the obstacle geometries alone, so an unobstructed route never pays for building it (~1 s on a large map).

Grid A*

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

8-connected A* over a cost grid; a step costs length * (1 + cost * grid_cost_weight).

Parameters:

Name Type Description Default
grid ndarray

(Y, X) cell costs, inf is blocked.

 required
start_utm tuple or ndarray

Start coordinates (easting, northing) in UTM.

 required
goal_utm tuple or ndarray

Goal coordinates (easting, northing) in UTM.

 required
low tuple of float

(min_x, min_y) of the grid in UTM.

 required
cs float

Cell size (m).

 required
grid_cost_weight float

Weight multiplier for non-zero cell costs (default 5.0).

 GRID_COST_WEIGHT

Returns:

Type Description
ndarray or None

Path of cell centers in world coordinates, or None if there is none.

RRT*

RRT* planner; asymptotically converges to the lowest-cost path.

A segment is blocked by an obstacle polygon or by a grid cell at or above traversability_threshold; otherwise the grid cost weighs its length, so the planner prefers low-cost corridors (e.g. footways) over open terrain.

__init__(start, goal, 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)

Set up the tree at start; :meth:find_path grows it.

Parameters:

Name Type Description Default
start ndarray

Start position [x, y].

 required
goal ndarray

Goal position [x, y].

 required
obstacles_tree STRtree or None

Index over the obstacle polygons; None checks the grid only.

 required
grid ndarray

(Y, X) cell costs; cells at or above traversability_threshold block.

 required
low tuple of float

(min_x, min_y) world coordinates of the grid.

 required
grid_scale float

Grid cell size (m).

 1.0
max_iter int

Maximum growth iterations.

 2000
step_size float

Longest extension per iteration, also the goal tolerance.

 2.0
neighbor_radius float

Rewiring radius (the upper bound when adaptive_radius).

 5.0
traversability_threshold float

Grid cells with cost at or above this value block.

 10.0
grid_cost_weight float

Weight of cell costs in segment scoring.

 5.0
transfer_id str or None

Planning stops when this transfer is cancelled.

 None
improve_after_goal bool

Keep rewiring for up to improve_iter iterations after the goal is first reached instead of returning; max_iter still caps the total.

 False
improve_iter int

Additional iterations after reaching the goal when improve_after_goal.

 200
informed bool

Once a solution exists, sample from its informed ellipse.

 True
adaptive_radius bool

Shrink the rewiring radius as the tree grows (RRT* optimality formula).

 True
rng Random or None

Sampling source; pass random.Random(seed) for a reproducible run.

 None

find_path()

Grow the tree for up to max_iter iterations and return the path.

Returns:

Type Description
ndarray or None

(N, 2) path, or None if none was found within the budget or the transfer was cancelled.

Smoothing

map_data.pathsolver.smoothing.smooth_path(path, collision_check_func=None, weight_data=0.5, weight_smooth=0.3, tolerance=0.001)

Gradient descent path smoothing.

Each sweep is a vectorised Jacobi update: every interior point is relaxed from the previous sweep's neighbours at once. collision_check_func is called once per sweep.

Parameters:

Name Type Description Default
path ndarray

The path to smooth as an (N, 2) or (N, 3) array.

 required
collision_check_func callable

Returns True if the LineString collides. On a collision the last collision-free sweep is returned.

 None
weight_data float

How much to weigh the original path points.

 0.5
weight_smooth float

How much to weigh the smoothness.

 0.3
tolerance float

Convergence tolerance, compared against the total absolute movement of a sweep.

 0.001

Returns:

Type Description
ndarray

The smoothed path.

Generic A*

astar_search works on any hashable node type; see Using astar_search directly.

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.