Path Planning

The pathsolver module provides standalone planning capabilities — no ROS2 context is required. Use the map_data_plan CLI to process waypoints against a .mapdata file, or import the planners directly as a Python library.

Offline route planning: `map_data_plan` and the `route_planner` action

The viewer's Planner screen (POST /api/create_replan), the map_data_plan command line tool and the route_planner ROS 2 action server all call the same function, map_data.pathsolver.route.plan_route, on the same map: the .mapdata file with its <stem>.annotations.json merged in (map_data.annotations.load_mapdata_with_annotations). Whatever is drawn, deleted or split in the viewer is therefore what the robot plans on. Both tools take an explicit annotation choice: --annotations auto|none|FILE on the command line, the annotations parameter (launch argument) on the node. none plans on the unedited OSM map, which is the right choice when the store was pruned for a different area than the one you are planning in (a heavily edited store can delete the very paths you need; the node logs the footway count and the number of deleted ways it applied).

Goal QR codes (Planner screen)

Robotour hands goals over as QR codes with a geo:lat,lon payload. In the Planner screen, right-click any waypoint and choose QR code, or press QR goal for the last waypoint: the code opens full screen on a white background (size slider) so a laptop or tablet can be held to the robot camera, with Download SVG (or PNG) for printing and Copy geo text for typing the goal in with qr_goal_send. The code comes from GET /api/qr.svg?lat=…&lon=…[&scale=12] [&download=1][&caption=] as vector art, so it stays sharp at any size and the caption under it is real text rather than pixels; GET /api/qr?… serves the same code as a PNG for anything that needs a bitmap (OpenCV QRCodeEncoder, map_data.utils.qr). An empty caption= leaves the caption off - the viewer draws its own as HTML text under the code.

Command line

# Paths only (graph planner on footways), 3 m waypoint spacing, saved as a GPX track
map_data_plan -f stromovka.mapdata --start 50.1038,14.4294 --goal geo:50.1067,14.4193 \
    --spacing 3 --save route.gpx

# All terrain (grid A*) through several points, printed as JSON
map_data_plan -f stromovka.mapdata -p 50.1038,14.4294 -p 50.1050,14.4250 -p 50.1067,14.4193 \
    --algorithm astar --cell-size 0.5 --json

--traversability FILE plans with another rule file than the package's config/traversability.yaml and --no-traversability with none at all (stairs are still excluded); see Traversability rules.

--goal accepts the geo:lat,lon URI printed on a Robotour QR code. Failures are reported with a reason: start_outside_map / goal_outside_map (the first / last point lies more than 100 m outside the map's downloaded area; for the start nearly always the wrong map file), snap_too_far (a point is farther than --max-snap-distance from every allowed way), unreachable (disconnected network), no_path, grid_too_large, too_few_points.

The graph planner returns on-network vertices only: a requested point that sits beside a path appears in the route as its projection onto that path, not as an extra vertex off to the side. (start_from_robot is the one exception, below.) The reported snap_distances say how far each request was moved.

ROS 2 action server

ros2 launch map_data route_planner.launch.py mapdata_file:=stromovka.mapdata
ros2 launch map_data route_planner.launch.py mapdata_file:=KN.mapdata highway_types:=footway,road

ros2 action send_goal /route_planner/plan_route map_data_interfaces/action/PlanRoute \
    "{waypoints: [{latitude: 50.1067, longitude: 14.4193}], start_from_robot: true}"

The node preloads its mapdata_file and keeps one graph planner per map, allowed-way set and snap distance (a few MB each, four at most), so a request plans in milliseconds; set preload:=false to load lazily. highway_types:= takes the way types as one comma- or space-separated string (footway, road or footway,road); a map with roads only plans nothing until roads are allowed.

Every parameter below lives in config/route_planner.yaml, which the launch file loads by default; params_file:= takes an absolute path or another file name in config/. The launch arguments (mapdata_file, mapdata_path, annotations, traversability, preload, algorithm, highway_types, spacing, mission_dir, gps_fix_topic, earth_frame, local_frame) are applied on top of that file, so an argument left unset keeps the file's value and the rest of the parameters are only reachable through the file. Not to be confused with config/planner_defaults.yaml, which holds the routing cost tables used by the planners themselves.

map_data_interfaces/action/PlanRoute takes the file, the ordered waypoints and the planner parameters (empty/zero fields use the node's defaults). With start_from_robot the latest fix on gps_fix_topic becomes the first waypoint, so a single goal is enough; that fix is kept verbatim as the route's first point (the robot is where it is), and the leading leg is its way onto the network. The goal is kept verbatim too (keep_goal), so the route ends at the requested coordinate and its last leg leaves the network; a goal farther than goal_max_snap_distance from every allowed way fails with snap_too_far rather than being planned to somewhere else. A start or goal more than outside_map_tolerance outside the map's area fails first (any algorithm) with start_outside_map / goal_outside_map, the message naming the loaded map file. The result carries the route as geographic_msgs/GeoPath, the same route as nav_msgs/Path in local_frame (through the earth_frame -> local_frame transform, exactly as osm_cloud places the map), the length, the snap distances and the path of the GPX track written to mission_dir. The route is also published latched on ~/route (GeoPath) and ~/route_path (Path) and a status string on ~/status. A geographic_msgs/GeoPointStamped published on ~/goal triggers the same planning from the robot's position with the default parameters.

Parameter	Default	Meaning
`mapdata_file`	`""`	default `.mapdata` (name in `data_dir` or absolute path)
`data_dir`	package `share/map_data/data`	where file names are resolved
`mission_dir`	`~/missions`	GPX output directory
`gps_fix_topic`	`/fixposition/odometry_llh`	`NavSatFix` for `start_from_robot`
`earth_frame`, `local_frame`	`FP_ECEF`, `FP_ENU0`	frames of `route_local`
`algorithm`	`graph`	`graph`, `astar` or `rrt`
`highway_types`	`["footway"]`	allowed way types: `footway`, `road` or both
`spacing`	`3.0`	max metres between output waypoints (0 = planner vertices)
`max_snap_distance`	`100.0`	graph: waypoint-to-way limit (m), the start's own
`goal_max_snap_distance`	`30.0`	graph: the goal's limit (m); farther fails with `snap_too_far`
`outside_map_tolerance`	`100.0`	m a start or goal may lie outside the map's area; farther fails with `start_outside_map` / `goal_outside_map`
`keep_goal`	`true`	end the route at the goal coordinate, not at its projection
`exclude_highway`	`["steps"]`	`highway=` values never routed over (stairs)
`traversability_file`	`""`	tag rule file deciding what may be driven on (`""` = the package's `config/traversability.yaml`, see Traversability rules); launch argument `traversability:=`
`cell_size`, `inflate_obstacles`	`0.25`, `0.25`	grid planners
`fix_max_age`	`10.0`	s after which the last fix is stale

Library

from map_data.annotations import load_mapdata_with_annotations
from map_data.pathsolver.route import RoutePlanningError, plan_route
from map_data.utils.gpx import create_gpx_track

md, _ = load_mapdata_with_annotations("data/stromovka.mapdata")
try:
    route = plan_route(md, [(50.1038, 14.4294), (50.1067, 14.4193)], spacing=3.0)
except RoutePlanningError as e:
    print(e.reason, e.message)
else:
    open("route.gpx", "w").write(create_gpx_track(route.latlon))

Traversability rules

Which OSM ways the robot may drive on is decided from their tags by config/traversability.yaml, the one file to edit when the answer changes. The rules are applied when a map is loaded for planning (load_mapdata_with_annotations), so the route planner, the osm_cloud cost cloud and the intersection rings all describe the same network; the saved .mapdata and the viewer keep showing everything.

default:
  traversable: true
  cost: 0.0
rules:
  - match: {highway: steps}          # exact value
    traversable: false
    reason: stairs
  - match: {surface: [grass, mud]}   # any of these values
    traversable: false
    reason: soft surface
  - match: {bridge: "*"}             # the tag is present, whatever its value
    traversable: false
    reason: bridge
  - match: {highway: path, informal: yes}   # several keys: all must match (AND)
    cost: 1.0
    reason: informal path

Rules are tried in order and the first match wins, so an allow rule placed before a deny rule is the way to make an exception ({bridge: boardwalk} traversable: true above {bridge: "*"} traversable: false). A way no rule matches gets default.
traversable: false removes the way: it is never routed over, its junctions get no intersection ring, and it is off-road in the osm_cloud cost cloud.
cost: is optional and additive on top of the highway/surface cost from config/planner_defaults.yaml: the graph planner weighs the way's edges (1 + way cost + rule cost) × length. Surfaces and way types are already priced by those tables, so use it only for tags they know nothing about (informal=yes). The reported route length is always the geometric one.
reason: is what the load log prints next to the count of removed ways.
YAML parses a bare yes/no as a boolean, but rule and tag values are compared as the strings OSM uses, so informal: yes and informal: "yes" are the same rule — quoting is never needed.
The file is validated on load: an unknown key, a negative cost or a non-boolean traversable is an error naming the file and the rule.

The shipped defaults refuse stairs, grass/mud/sand surfaces, bridges, smoothness=bad or worse and access=no|private. Measured on the Stromovka map (kralovska_obora.mapdata, 289 ways) they remove 15 stairways, 6 soft-surface ways, 13 bridges and 1 rough way, and leave both routes driven there on 2026-09-08 exactly as they were. A tunnel rule is shipped commented out: the covered alley at Šlechtova is the only short way west, and removing the two tunnels turned a 751 m route into 2495 m.

Where the file is chosen:


`route_planner`, `osm_cloud`	parameter `traversability_file` (`""` = the package file)
launch files	`traversability:=<file>` (forwarded only when non-empty)
`map_data_plan`	`--traversability FILE`, `--no-traversability`
library	`load_mapdata_with_annotations(..., traversability=...)`, `plan_route(..., traversability=...)`, `GraphPlanner(..., traversability=...)` — a `TraversabilityRules`, a path, or `None` for the package file

route_planner includes the file's mtime in its map and planner caches, so an edit is picked up by the next goal. osm_cloud reads it once at startup: edit the file, restart that node. Both nodes must be given the same file, or osm_cloud publishes rings on ways the planner refuses.

Python Library

All planners work standalone — no ROS2 context required.

GraphPlanner

Plans a route constrained to the OSM road and footway network using A*.

from map_data.map_data import MapData
from map_data.pathsolver.graph_planner import GraphPlanner
import numpy as np

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

start = np.array([md.min_x + 10, md.min_y + 10])
goal = np.array([md.max_x - 10, md.max_y - 10])

planner = GraphPlanner(md, highway_types=["footway", "road"])
result = planner.plan(np.array([start, goal]))  # np.ndarray or None

ReplanPath

Grid-based local replanning around OSM barriers. See the ReplanPath API reference for full details.

import copy

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

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

args = copy.copy(DEFAULT_ARGS)
args.low = (md.min_x, md.min_y)
args.high = (md.max_x, md.max_y)
args.cell_size = 0.25
args.inflate_obstacles = 0.25
args.simplify_path = True
args.smooth_path = False

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

Available Algorithms

Graph Planning

Plans a path by searching the OSM road and footway network using A*. The route is constrained to follow existing ways, making it suitable for on-road or on-path navigation where staying on designated routes is required. This algorithm is fast and produces geometrically clean results, but cannot leave the road network to avoid obstacles. Its edge weights are length × (1 + way cost) with the same highway_costs/surface_costs tables the cost grid uses, plus any extra cost from the traversability rules, so a gravel shortcut loses to a slightly longer paved way; the reported route length stays geometric.

Walkable areas — closed area=yes (or multipolygon) ways such as pedestrian squares — are crossed, not walked around. Their entries are the network nodes on, inside or within 1 m of the area; between two entries the planner takes the shortest path inside the polygon, bending round concave corners and holes. These crossings are worked out per area when the search first reaches it and are never added to the graph. A waypoint inside an area stays where it is (its snap distance is 0) instead of moving to the area's rim.

Grid A*

Discretizes the area around the route into a uniform grid and runs A* on it. Each cell is marked free or occupied based on OSM barrier polygons (optionally inflated). Grid A* is the recommended all-terrain algorithm: it is complete (finds a path if one exists), produces near-optimal paths, and has predictable runtime behavior. Grid resolution and obstacle inflation are tunable via --cell_size and --inflate_obstacles.

RRT*

Rapidly-exploring Random Tree Star is a sampling-based planner that builds a tree of collision-free waypoints by randomly sampling the free space. It is asymptotically optimal — given enough iterations it converges to the shortest path — and handles irregular obstacle shapes well. RRT* is best suited for large open areas or when the grid resolution required for Grid A* would be prohibitively expensive.

Cost grid

ReplanPath.fill_grid() converts OSM map data into a 2-D NumPy array of floating-point costs, one value per grid cell. Each cell represents a cell_size × cell_size metre patch of ground.

Cell cost assignment:

Barrier polygons (inflated by inflate_obstacles) are rasterised as impassable (cost = inf).
For every remaining cell, the distance to the nearest way centre-line is calculated.
If that distance is ≤ max_path_dist, the cell receives a blended cost:
```
cost = way_cost + (default_off_path_cost − way_cost) × (dist / max_path_dist)²
```
where way_cost = min(path_cost_cap, highway_cost + surface_cost).
Cells beyond max_path_dist from any way receive default_off_path_cost (default 0.9).

Costs run from 0.0 (freely preferred, e.g. a pedestrian footway on asphalt) to 1.0 (impassable). The planner treats cells with cost ≥ 1.0 as obstacles. See Planner Configuration for the full cost tables.

Traversability threshold. Both Grid A* and RRT* consider a cell passable if its cost is < 1.0. There is no separate traversability threshold parameter — adjust inflate_obstacles to shrink or expand the impassable zone around physical barriers, and adjust default_off_path_cost to make off-path terrain more or less discouraged.

Performance tuning

Grid resolution (`cell_size`)

cell_size is the dominant factor in both memory use and planning time.

`cell_size`	Grid cells per 100 m²	Typical use case
`0.5 m`	400	Coarse survey, large open areas
`0.25 m`	1 600	Default — good balance for urban pedestrian routes
`0.1 m`	10 000	Tight corridors, narrow gates

Halving cell_size quadruples the number of cells and roughly doubles planning time. For routes longer than a few hundred metres, prefer 0.25 m or 0.5 m and rely on inflate_obstacles to enforce clearance rather than resolving every surface detail at fine resolution.

Obstacle inflation (`inflate_obstacles`)

Increasing inflate_obstacles adds a safety margin around barriers at the cost of blocking routes through narrow gaps. If the planner returns False (no path found), try reducing this value before increasing grid resolution.

Path post-processing

simplify_path (Douglas-Peucker) is cheap and almost always beneficial — it reduces the waypoint count without noticeably changing the path shape. Enable it for any path that will be sent to a navigation stack.

smooth_path applies gradient-descent smoothing. It rounds sharp corners but can shift waypoints slightly off the low-cost cells produced by the planner. Use it when the downstream controller benefits from smooth curvature and the small positional deviation is acceptable.

Algorithm choice

Situation	Recommended algorithm
Well-mapped pedestrian network, path must stay on ways	`GraphPlanner`
Urban route, obstacles well-defined by OSM barriers	Grid A*
Large open area, sparse obstacles	RRT*
Very large area where a fine Grid A* grid is too expensive	RRT*

Using `astar_search` directly

astar_search is the generic A* implementation that powers both GraphPlanner and grid_astar. You can use it directly to build a custom planner for any graph-like problem.

from map_data.pathsolver.astar import astar_search


# Example: plan over a simple weighted grid
def neighbors(node):
    r, c = node
    candidates = [(r - 1, c), (r + 1, c), (r, c - 1), (r, c + 1)]
    return [(n, 1.0) for n in candidates if 0 <= n[0] < 10 and 0 <= n[1] < 10]


def heuristic(node):
    goal = (9, 9)
    return abs(node[0] - goal[0]) + abs(node[1] - goal[1])


path = astar_search(
    start_node=(0, 0),
    goal_node=(9, 9),
    get_neighbors_func=neighbors,
    heuristic_func=heuristic,
)
# path is a list of (row, col) tuples from start to goal, or None if unreachable

get_neighbors_func must return an iterable of (neighbor_node, edge_cost) tuples. heuristic_func must be admissible (never overestimate the true cost to the goal) for A* to return an optimal path.