diff --git a/README.md b/README.md
index 19710ac..f003073 100644
--- a/README.md
+++ b/README.md
@@ -347,6 +347,18 @@ ros2 run navmap_tools navmap_simplify in.navmap out.navmap --max-error 0.02 \
---
+## 7h-bis. A NavMap of any place, with its buildings (`navmap_gis_tool`)
+
+`navmap_gis_tool` samples the terrain of a square around a GPS point (IGN in Spain, Copernicus elsewhere) on a grid. `--navmap-out` writes it as a NavMap, two NavCels per cell, without the cells that have no ground or are steeper than `--max-slope-deg` (vectorised: tens of millions of cells in seconds). `--buildings` downloads the footprints of the buildings and leaves out every cell any part of a building is in: `catastro` (Spain, the Catastro INSPIRE WFS, building parts with floors above ground, patios kept as ground), `osm` (OpenStreetMap, anywhere) or `auto` (the Catastro in Spain, OpenStreetMap elsewhere or where the Catastro has none). The answers are cached on disk.
+
+```bash
+# 2 x 2 km around the Puerta del Sol (Madrid), cells of 0.5 m
+ros2 run navmap_tools navmap_gis_tool --center 40.41690,-3.70350 --size 2000 --resolution 0.5 \
+ --dem-source ign --buildings auto --navmap-out sol.navmap
+```
+
+---
+
## 7i. Clouds with sensor positions (`from_points` with viewpoints)
A cloud whose points keep the position of the sensor that saw them (`pcl::PointWithViewpoint`: `vp_x`, `vp_y`, `vp_z`) tells free space from occupied space, which a bare cloud cannot. `from_points` takes it and cleans the cloud before meshing (`filter_with_viewpoints`):
diff --git a/navmap_tools/navmap_tools/cli.py b/navmap_tools/navmap_tools/cli.py
index 57a612d..3d17798 100644
--- a/navmap_tools/navmap_tools/cli.py
+++ b/navmap_tools/navmap_tools/cli.py
@@ -33,12 +33,14 @@
import numpy as np
+from .geo.buildings import describe, fetch_buildings, footprint_cells
from .geo.cache import default_cache_dir, DiskCache
from .geo.dem import load_dem_grid
from .geo.dem_file import (
add_imagery, build_local_dem, DemSource, read_dem_file, SyntheticDem, SyntheticImagery,
write_dem_file,
)
+from .geo.dem_navmap import navmap_from_dem
from .geo.google import (
API_KEY_ENV_VAR as GOOGLE_API_KEY_ENV_VAR, get_api_key as get_google_api_key,
load_google_mosaic,
@@ -51,6 +53,7 @@
from .geo.terrain_export import write_gazebo_terrain, write_world_from_template
from .mesh_export import write_dae, write_stl, write_texture_png
from .pcd_writer import write_colors_csv, write_pcd_xyz
+from .resample import write_navmap
from .scaffold import scaffold_world_package
from .terrain import build_terrain_grid, render_texture
@@ -195,9 +198,25 @@ def build_arg_parser() -> argparse.ArgumentParser:
parser.add_argument(
'--dem-holes-mesh', default=None, metavar='STL',
help=(
- '--dem-out: STL in the local frame of --center (buildings, walls); the cells its '
- 'triangles touch (seen from above) have no ground in the DEM: holes, as the walls of '
- 'a NavMap. Use it with a --mesh-file of the ground alone'
+ '--dem-out / --navmap-out: STL in the local frame of --center (buildings, walls); the '
+ 'cells its triangles touch (seen from above) have no ground in the DEM: holes, as the '
+ 'walls of a NavMap. Use it with a --mesh-file of the ground alone'
+ ))
+ parser.add_argument(
+ '--buildings', choices=('auto', 'catastro', 'osm'), default=None,
+ help=(
+ '--dem-out / --navmap-out: download the footprints of the buildings and leave their '
+ 'cells without ground (a cell any part of a building is in): catastro (Spain, the '
+ 'Catastro INSPIRE WFS), osm (OpenStreetMap, anywhere), auto (the Catastro in Spain, '
+ 'OpenStreetMap elsewhere or where the Catastro has none)'
+ ))
+ parser.add_argument(
+ '--navmap-out', default=None, metavar='FILE',
+ help=(
+ 'write the local height grid as a .navmap: two NavCels per cell of --resolution m, '
+ 'without the cells that have no ground (--buildings, --dem-holes-mesh) or are steeper '
+ 'than --max-slope-deg; vertex colours with --dem-imagery. Vectorised, for large areas '
+ '(unlike --navmap)'
))
parser.add_argument(
'--mesh-world-elevation', type=float, default=None, metavar='METERS',
@@ -254,8 +273,8 @@ def run(argv=None) -> int:
args = parser.parse_args(argv)
_mesh_heightfield.cache_clear() # once per run: the file may have changed since the last
- if not args.gazebo and not args.navmap and not args.dem_out:
- parser.error('at least one of --gazebo/--navmap/--dem-out is required')
+ if not args.gazebo and not args.navmap and not args.dem_out and not args.navmap_out:
+ parser.error('at least one of --gazebo/--navmap/--dem-out/--navmap-out is required')
if args.size <= 0:
parser.error('--size must be positive')
if args.resolution <= 0:
@@ -273,8 +292,8 @@ def run(argv=None) -> int:
parser.error('--imagery-source xyz needs --imagery-url')
if args.dem_source == 'mesh' and not args.mesh_file:
parser.error('--dem-source mesh needs --mesh-file')
- if args.dem_holes_mesh and not args.dem_out:
- parser.error('--dem-holes-mesh needs --dem-out')
+ if (args.dem_holes_mesh or args.buildings) and not (args.dem_out or args.navmap_out):
+ parser.error('--dem-holes-mesh and --buildings need --dem-out or --navmap-out')
if args.dem_source == 'mesh' and args.mesh_cell <= 0:
parser.error('--mesh-cell must be positive')
if args.dem_data_resolution < 0:
@@ -290,12 +309,13 @@ def run(argv=None) -> int:
# the world / NavMap sampling loop is pure Python; --dem-out alone is vectorised
needs_grid = args.gazebo or args.navmap
- if args.dem_out:
+ if args.dem_out or args.navmap_out:
need = _dem_memory_bytes(args)
available = _available_memory_bytes()
if available is not None and need > available:
parser.error(
- f'--dem-out at --size {args.size} / --resolution {args.resolution} needs about '
+ f'--dem-out/--navmap-out at --size {args.size} / --resolution {args.resolution} '
+ f'needs about '
f'{need / 1e9:.1f} GB to build and {available / 1e9:.1f} GB are available; '
'increase --resolution or reduce --size')
n_per_side = round(args.size / args.resolution) + 1
@@ -335,8 +355,8 @@ def run(argv=None) -> int:
f"than the DEM's native ~{dem_native_m:.1f} m resolution; elevation detail "
'below that will be smoothly interpolated, not real', file=sys.stderr)
- if args.dem_out:
- _write_dem(args, dem, imagery, lat, lon)
+ if args.dem_out or args.navmap_out:
+ _write_dem(args, dem, imagery, lat, lon, cache)
grid = build_terrain_grid(projection, dem, imagery, args.size, args.resolution)
paths = scaffold_world_package(
@@ -518,7 +538,7 @@ def _run_dem_only(args, bbox, cache) -> int:
lat, lon = args.center
radius, reference, data_res, mesh = _dem_geometry(args)
grid = None
- if not args.force_refresh and Path(args.dem_out).is_file():
+ if args.dem_out and not args.force_refresh and Path(args.dem_out).is_file():
try:
old = read_dem_file(args.dem_out)
except (OSError, ValueError):
@@ -541,12 +561,14 @@ def _run_dem_only(args, bbox, cache) -> int:
source=_source_id(args), reference_altitude=reference, data_resolution=data_res)
if imagery is not None:
add_imagery(grid, imagery, args.zoom)
- _add_holes(grid, args)
- _write_dem_and_inputs(grid, args)
- print(
- f'[navmap_gis_tool] wrote {args.dem_out} ({grid.side}x{grid.side} nodes of '
- f'{grid.resolution} m, sampled every {grid.data_resolution} m, '
- f'reference altitude {grid.reference_altitude:.1f} m)')
+ _add_holes(grid, args, cache)
+ if args.dem_out:
+ _write_dem_and_inputs(grid, args)
+ print(
+ f'[navmap_gis_tool] wrote {args.dem_out} ({grid.side}x{grid.side} nodes of '
+ f'{grid.resolution} m, sampled every {grid.data_resolution} m, '
+ f'reference altitude {grid.reference_altitude:.1f} m)')
+ _write_navmap_out(grid, args)
if args.terrain_out:
write_gazebo_terrain(grid, args.terrain_out, args.terrain_resolution)
print(f'[navmap_gis_tool] wrote the Gazebo terrain in {args.terrain_out}')
@@ -569,7 +591,8 @@ def _file_id(path):
def _dem_inputs(args) -> dict:
"""Return what makes a DEM besides its geometry: the input files and options, the imagery."""
- dem = {'dem_source': args.dem_source, 'holes_mesh': _file_id(args.dem_holes_mesh)}
+ dem = {'dem_source': args.dem_source, 'holes_mesh': _file_id(args.dem_holes_mesh),
+ 'buildings': args.buildings}
if args.dem_source == 'mesh':
dem.update(mesh_file=_file_id(args.mesh_file), mesh_cell=args.mesh_cell,
mesh_world_elevation=args.mesh_world_elevation)
@@ -598,7 +621,7 @@ def _is_the_same(old, args, lat, lon, radius, reference, data_res) -> bool:
"""Whether the DEM file on disk is what these arguments would make."""
if not old.matches(lat, lon, radius, args.resolution, _source_id(args), reference, data_res):
return False
- if bool(args.dem_holes_mesh) != (old.cell_valid is not None):
+ if bool(args.dem_holes_mesh or args.buildings) != (old.cell_valid is not None):
return False
try: # made from the same files and options (a DEM without this record is rebuilt)
recorded = json.loads(_inputs_path(args.dem_out).read_text())
@@ -612,30 +635,58 @@ def _is_the_same(old, args, lat, lon, radius, reference, data_res) -> bool:
recorded.get('imagery') == wanted['imagery'])
-def _add_holes(grid, args) -> None:
- """--dem-holes-mesh: the cells its triangles touch have no ground."""
- if not args.dem_holes_mesh:
+def _add_holes(grid, args, cache=None) -> None:
+ """
+ Leave without ground the cells of the obstacles.
+
+ --dem-holes-mesh: the cells its triangles touch; --buildings: those of the footprints.
+ """
+ if not args.dem_holes_mesh and not args.buildings:
return
- holes = hole_cells(load_stl_triangles(args.dem_holes_mesh), grid.side, grid.resolution)
+ nc = grid.side - 1
+ holes = np.zeros((nc, nc), dtype=bool)
+ if args.dem_holes_mesh:
+ holes |= hole_cells(load_stl_triangles(args.dem_holes_mesh), grid.side, grid.resolution)
+ print(f'[navmap_gis_tool] holes from {args.dem_holes_mesh}: {holes.sum()} cells')
+ if args.buildings:
+ if cache is None:
+ cache = DiskCache(args.cache_dir or default_cache_dir())
+ footprints, source = fetch_buildings(
+ args.buildings, grid.origin_lat, grid.origin_lon, grid.radius, cache)
+ before = int(holes.sum())
+ footprint_cells(footprints, grid.side, grid.resolution, holes)
+ print(f'[navmap_gis_tool] buildings ({source}): {describe(footprints)}, '
+ f'{int(holes.sum()) - before} cells')
grid.cell_valid = (~holes).astype(np.uint8)
- print(f'[navmap_gis_tool] holes from {args.dem_holes_mesh}: {holes.sum()} of {holes.size} '
- f'cells without ground')
+ print(f'[navmap_gis_tool] {int(holes.sum())} of {holes.size} cells without ground')
+
+
+def _write_navmap_out(grid, args) -> None:
+ """--navmap-out: the local grid as a NavMap."""
+ if not args.navmap_out:
+ return
+ msg = navmap_from_dem(grid, args.max_slope_deg)
+ write_navmap(msg, args.navmap_out)
+ print(f'[navmap_gis_tool] wrote {args.navmap_out} ({len(msg.navcels_v0)} NavCels, '
+ f'{len(msg.positions_x)} vertices)')
-def _write_dem(args, dem, imagery, lat, lon) -> None:
- """--dem-out next to a world / NavMap: the local height grid that bigmap_maps_manager reads."""
+def _write_dem(args, dem, imagery, lat, lon, cache=None) -> None:
+ """--dem-out / --navmap-out next to a world / NavMap: the local height grid."""
radius, reference, data_res, _ = _dem_geometry(args)
grid = build_local_dem(
dem, lat, lon, radius=radius, resolution=args.resolution,
source=_source_id(args), reference_altitude=reference, data_resolution=data_res)
if imagery is not None and args.dem_imagery:
add_imagery(grid, imagery, args.zoom) # the zoom the DEM-only path compares with
- _add_holes(grid, args)
- _write_dem_and_inputs(grid, args)
- print(
- f'[navmap_gis_tool] wrote {args.dem_out} ({grid.side}x{grid.side} nodes of '
- f'{grid.resolution} m, sampled every {grid.data_resolution} m, '
- f'reference altitude {grid.reference_altitude:.1f} m)')
+ _add_holes(grid, args, cache)
+ if args.dem_out:
+ _write_dem_and_inputs(grid, args)
+ print(
+ f'[navmap_gis_tool] wrote {args.dem_out} ({grid.side}x{grid.side} nodes of '
+ f'{grid.resolution} m, sampled every {grid.data_resolution} m, '
+ f'reference altitude {grid.reference_altitude:.1f} m)')
+ _write_navmap_out(grid, args)
if args.terrain_out:
write_gazebo_terrain(grid, args.terrain_out, args.terrain_resolution)
if args.world_template:
diff --git a/navmap_tools/navmap_tools/geo/buildings.py b/navmap_tools/navmap_tools/geo/buildings.py
new file mode 100644
index 0000000..54de6be
--- /dev/null
+++ b/navmap_tools/navmap_tools/geo/buildings.py
@@ -0,0 +1,309 @@
+# Copyright 2026 Intelligent Robotics Lab
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+The footprints of the buildings of an area, from open data, as cells without ground of a grid.
+
+ * Spain: the Catastro, through its INSPIRE buildings WFS (no key): every building part with
+ its footprint (and patios) and its number of floors above ground. The service answers
+ nothing for boxes over about 1 km^2, so the area is asked in tiles.
+ * Anywhere else, and in Spain where the Catastro has nothing (the Basque Country and Navarra
+ keep their own cadastre): OpenStreetMap, through the Overpass API (no key).
+
+Every answer is kept in the disk cache: a second run of the same area asks nothing.
+"""
+
+import json
+import math
+import time
+from typing import List, Optional, Tuple
+import xml.etree.ElementTree as ET
+
+import numpy as np
+from pyproj import CRS, Transformer
+import requests
+
+from .cache import DiskCache
+from .dem_file import EnuProjection
+
+CATASTRO_URL = 'https://ovc.catastro.meh.es/INSPIRE/wfsBU.aspx'
+OVERPASS_URLS = ('https://overpass-api.de/api/interpreter',
+ 'https://overpass.private.coffee/api/interpreter',
+ 'https://overpass.kumi.systems/api/interpreter')
+_NS = {
+ 'gml': 'http://www.opengis.net/gml/3.2',
+ 'bu': 'http://inspire.jrc.ec.europa.eu/schemas/bu-ext2d/2.0',
+}
+_HEADERS = {'User-Agent': 'navmap_tools'}
+# OpenStreetMap building=* that are no obstacle at ground level (a roof on pillars, a carport)
+_OSM_NOT_AN_OBSTACLE = ('no', 'roof', 'carport')
+
+# A footprint: exterior ring (N, 2) and its holes (patios), in the local ENU frame (m)
+Footprint = Tuple[np.ndarray, List[np.ndarray]]
+
+
+def in_spain(lat: float, lon: float) -> bool:
+ """Whether the Catastro may hold (lat, lon): the peninsula, the islands, Ceuta and Melilla."""
+ return ((35.0 <= lat <= 44.0 and -7.5 <= lon <= 5.0) or
+ (41.7 <= lat <= 44.0 and -9.4 <= lon <= -6.0) or
+ (27.0 <= lat <= 29.5 and -19.0 <= lon <= -13.0))
+
+
+def _area_lonlat(enu: EnuProjection, radius: float, pad: float):
+ """(west, south, east, north) degrees of the square of half side radius + pad."""
+ r = radius + pad
+ lat, lon, _ = enu.to_geodetic(np.array([-r, r, r, -r]), np.array([-r, -r, r, r]))
+ return float(lon.min()), float(lat.min()), float(lon.max()), float(lat.max())
+
+
+def _cached_get(cache: DiskCache, url: str, params: dict, what: str, timeout=120.0) -> bytes:
+ key = url + '?' + json.dumps(params, sort_keys=True)
+
+ def fetch(dest):
+ last = None
+ for attempt in range(4):
+ if attempt:
+ time.sleep(2.0 ** attempt) # 2, 4, 8 s: the services are often busy
+ try:
+ r = requests.get(url, params=params, headers=_HEADERS, timeout=timeout)
+ if r.status_code == 200:
+ dest.write_bytes(r.content)
+ return
+ last = f'HTTP {r.status_code}'
+ except requests.RequestException as e:
+ last = str(e)
+ print(f'[navmap_gis_tool] {what}: attempt {attempt + 1} failed ({last})')
+ raise RuntimeError(f'{what}: {last}')
+ return cache.get_or_fetch(key, '.xml', fetch).read_bytes()
+
+
+def fetch_catastro(lat0: float, lon0: float, radius: float, cache: DiskCache,
+ tile: float = 800.0) -> List[Footprint]:
+ """Footprints of the Catastro's building parts with floors above ground."""
+ enu = EnuProjection(lat0, lon0)
+ epsg = 4083 if lat0 < 30.0 else 25830 # REGCAN95 UTM 28N (Canary Islands) / ETRS89 UTM 30N
+ to_proj = Transformer.from_crs(CRS.from_epsg(4326), CRS.from_epsg(epsg), always_xy=True)
+ to_ll = Transformer.from_crs(CRS.from_epsg(epsg), CRS.from_epsg(4326), always_xy=True)
+ w, s, e, n = _area_lonlat(enu, radius, 20.0)
+ xs, ys = to_proj.transform([w, e, e, w], [s, s, n, n])
+ x0, y0, x1, y1 = min(xs), min(ys), max(xs), max(ys)
+ nx, ny = max(1, math.ceil((x1 - x0) / tile)), max(1, math.ceil((y1 - y0) / tile))
+
+ def local(ring):
+ lon, lat = to_ll.transform(ring[:, 0], ring[:, 1])
+ east, north, _ = enu.to_enu(np.asarray(lat), np.asarray(lon))
+ return np.c_[east, north]
+
+ out, seen = [], set()
+
+ def get(tb, name, typename):
+ return _cached_get(cache, CATASTRO_URL, {
+ 'service': 'wfs', 'version': '2.0.0', 'request': 'GetFeature',
+ 'typenames': typename, 'srsname': f'EPSG::{epsg}',
+ 'bbox': '%.2f,%.2f,%.2f,%.2f,urn:ogc:def:crs:EPSG::%d' % (tuple(tb) + (epsg,))},
+ f'Catastro buildings, tile {name}')
+
+ def ask(tb, name, depth=0):
+ """Ask the building parts of a tile; one the service keeps failing on, in four."""
+ try:
+ body = get(tb, name, 'BU:BuildingPart')
+ except RuntimeError:
+ if depth >= 2:
+ # the service fails on the parts of some places (an error page) but serves their
+ # buildings: their footprints, without the floors
+ print(f'[navmap_gis_tool] Catastro buildings, tile {name}: whole buildings')
+ body = get(tb, name + ' (buildings)', 'BU:Building')
+ parse(ET.fromstring(body), 'Building')
+ return
+ mx, my = (tb[0] + tb[2]) / 2, (tb[1] + tb[3]) / 2
+ print(f'[navmap_gis_tool] Catastro buildings, tile {name}: asked in four')
+ for k, sub in enumerate(((tb[0], tb[1], mx, my), (mx, tb[1], tb[2], my),
+ (tb[0], my, mx, tb[3]), (mx, my, tb[2], tb[3]))):
+ ask(sub, f'{name}.{k + 1}', depth + 1)
+ return
+ try:
+ root = ET.fromstring(body)
+ except ET.ParseError as ex:
+ raise RuntimeError(f'Catastro buildings: not XML ({ex})')
+ if root.tag.endswith('ExceptionReport'):
+ if b'No records' in body:
+ return # nothing there (rural, or a territory it does not hold)
+ raise RuntimeError('Catastro buildings: ' + body[:300].decode(errors='replace'))
+ parse(root, 'BuildingPart')
+
+ def parse(root, element):
+ for part in root.iter('{%s}%s' % (_NS['bu'], element)):
+ pid = part.get('{%s}id' % _NS['gml'], '')
+ if pid in seen:
+ continue
+ seen.add(pid)
+ floors = part.find('.//bu:numberOfFloorsAboveGround', _NS)
+ if element == 'BuildingPart' and (
+ floors is None or not (floors.text or '').strip().isdigit() or
+ int(floors.text) < 1):
+ continue # underground, or a part with nothing above the ground
+ for patch in part.iter('{%s}PolygonPatch' % _NS['gml']):
+ ext = patch.find('gml:exterior', _NS)
+ if ext is None:
+ continue
+ try:
+ rings = [_pos_list(ext)] + [_pos_list(h)
+ for h in patch.findall('gml:interior', _NS)]
+ except (ValueError, AttributeError):
+ continue # a malformed polygon
+ out.append((local(rings[0]), [local(r) for r in rings[1:]]))
+
+ for j in range(ny):
+ for i in range(nx):
+ ask((x0 + (x1 - x0) * i / nx, y0 + (y1 - y0) * j / ny,
+ x0 + (x1 - x0) * (i + 1) / nx, y0 + (y1 - y0) * (j + 1) / ny),
+ f'{j * nx + i + 1}/{nx * ny}')
+ return out
+
+
+def _pos_list(el) -> np.ndarray:
+ v = np.array(el.find('.//gml:posList', _NS).text.split(), dtype=np.float64).reshape(-1, 2)
+ return v[:-1] if len(v) > 1 and np.allclose(v[0], v[-1]) else v
+
+
+def fetch_osm(lat0: float, lon0: float, radius: float, cache: DiskCache) -> List[Footprint]:
+ """Footprints of the OpenStreetMap buildings (ways and multipolygons)."""
+ enu = EnuProjection(lat0, lon0)
+ w, s, e, n = _area_lonlat(enu, radius, 20.0)
+ query = ('[out:json][timeout:180];(way["building"](%f,%f,%f,%f);'
+ 'relation["building"]["type"="multipolygon"](%f,%f,%f,%f););out geom tags;'
+ % ((s, w, n, e) * 2))
+
+ def fetch(dest):
+ last = None
+ for k in range(6):
+ try:
+ r = requests.post(OVERPASS_URLS[k % len(OVERPASS_URLS)], data={'data': query},
+ headers=_HEADERS, timeout=200)
+ if r.status_code == 200:
+ json.loads(r.content) # whole: not an error page
+ dest.write_bytes(r.content)
+ return
+ last = f'HTTP {r.status_code}'
+ except (requests.RequestException, ValueError) as ex:
+ last = str(ex)
+ print(f'[navmap_gis_tool] OpenStreetMap buildings: attempt {k + 1} failed ({last})')
+ raise RuntimeError(f'OpenStreetMap buildings: {last}')
+
+ data = json.loads(cache.get_or_fetch('overpass:' + query, '.json', fetch).read_bytes())
+
+ def local(points):
+ lon = np.array([p['lon'] for p in points])
+ lat = np.array([p['lat'] for p in points])
+ east, north, _ = enu.to_enu(lat, lon)
+ ring = np.c_[east, north]
+ return ring[:-1] if len(ring) > 1 and np.allclose(ring[0], ring[-1]) else ring
+
+ out = []
+ for el in data.get('elements', []):
+ if el.get('tags', {}).get('building', 'yes') in _OSM_NOT_AN_OBSTACLE:
+ continue
+ if el['type'] == 'way':
+ g = el.get('geometry') or []
+ if len(g) >= 4:
+ out.append((local(g), []))
+ else:
+ outer = [local(m['geometry']) for m in el.get('members', [])
+ if m['type'] == 'way' and m.get('role') == 'outer' and m.get('geometry')]
+ inner = [local(m['geometry']) for m in el.get('members', [])
+ if m['type'] == 'way' and m.get('role') == 'inner' and m.get('geometry')]
+ for ring in outer:
+ if len(ring) >= 3:
+ out.append((ring, [h for h in inner if len(h) >= 3 and
+ _inside(ring, h[:1, 0], h[:1, 1])[0]]))
+ return out
+
+
+def fetch_buildings(source: str, lat0: float, lon0: float, radius: float,
+ cache: DiskCache) -> Tuple[List[Footprint], str]:
+ """Footprints from `source` (catastro, osm or auto) and the name of what answered."""
+ if source == 'catastro' or (source == 'auto' and in_spain(lat0, lon0)):
+ parts = fetch_catastro(lat0, lon0, radius, cache)
+ if parts or source == 'catastro':
+ return parts, 'Catastro'
+ return fetch_osm(lat0, lon0, radius, cache), 'OpenStreetMap'
+
+
+def _inside(ring: np.ndarray, x: np.ndarray, y: np.ndarray) -> np.ndarray:
+ """Whether the points (x, y) are inside the polygon `ring` (even-odd rule)."""
+ inside = np.zeros(np.shape(x), dtype=bool)
+ xs, ys = ring[:, 0], ring[:, 1]
+ x2, y2 = np.roll(xs, -1), np.roll(ys, -1)
+ for ax, ay, bx, by in zip(xs, ys, x2, y2):
+ if ay == by:
+ continue
+ crosses = ((ay > y) != (by > y)) & (x < (bx - ax) * (y - ay) / (by - ay) + ax)
+ inside ^= crosses
+ return inside
+
+
+def footprint_cells(footprints: List[Footprint], side: int, resolution: float,
+ holes: Optional[np.ndarray] = None) -> np.ndarray:
+ """
+ Mark the cells of a local grid that a building stands on.
+
+ Returns (side - 1, side - 1) bool, [j, i], for the grid of `dem_file.LocalDem` (nodes at
+ (i - (side - 1) / 2) * resolution, cell (i, j) between nodes i..i+1 and j..j+1). A cell is
+ marked if its centre is inside a footprint (not in one of its patios) or if an edge of the
+ footprint or of a patio crosses it: the rule of a collision map, a cell is blocked if any
+ part of a building is in it. `holes`, if given, is updated and returned.
+ """
+ nc = side - 1
+ out = holes if holes is not None else np.zeros((nc, nc), dtype=bool)
+ half = (side - 1) // 2
+
+ def cell_of(v):
+ return np.floor(v / resolution + half).astype(np.int64)
+
+ for ext, patios in footprints:
+ if len(ext) < 3:
+ continue
+ lo, hi = cell_of(ext.min(axis=0)), cell_of(ext.max(axis=0))
+ if hi[0] < 0 or hi[1] < 0 or lo[0] >= nc or lo[1] >= nc:
+ continue
+ i0, j0 = max(lo[0], 0), max(lo[1], 0)
+ i1, j1 = min(hi[0], nc - 1), min(hi[1], nc - 1)
+ cj, ci = np.mgrid[j0:j1 + 1, i0:i1 + 1]
+ cx, cy = (ci - half + 0.5) * resolution, (cj - half + 0.5) * resolution
+ inside = _inside(ext, cx, cy)
+ for p in patios:
+ if len(p) >= 3:
+ inside &= ~_inside(p, cx, cy)
+ out[cj[inside], ci[inside]] = True
+ # the cells the edges cross (sampled at a quarter of a cell)
+ for ring in [ext] + [p for p in patios if len(p) >= 3]:
+ a, b = ring, np.roll(ring, -1, axis=0)
+ for (ax, ay), (bx, by) in zip(a, b):
+ steps = max(1, int(math.ceil(math.hypot(bx - ax, by - ay) / (resolution / 4))))
+ t = np.linspace(0.0, 1.0, steps + 1)
+ pi = cell_of(ax + t * (bx - ax))
+ pj = cell_of(ay + t * (by - ay))
+ keep = (pi >= 0) & (pj >= 0) & (pi < nc) & (pj < nc)
+ out[pj[keep], pi[keep]] = True
+ return out
+
+
+def describe(footprints: List[Footprint]) -> str:
+ """'N buildings (M patios)'."""
+ patios = sum(len(p) for _, p in footprints)
+ return f'{len(footprints)} footprints ({patios} patios)'
+
+
+__all__ = ['fetch_buildings', 'fetch_catastro', 'fetch_osm', 'footprint_cells', 'in_spain',
+ 'describe', 'Footprint']
diff --git a/navmap_tools/navmap_tools/geo/dem_navmap.py b/navmap_tools/navmap_tools/geo/dem_navmap.py
new file mode 100644
index 0000000..ad08333
--- /dev/null
+++ b/navmap_tools/navmap_tools/geo/dem_navmap.py
@@ -0,0 +1,102 @@
+# Copyright 2026 Intelligent Robotics Lab
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+A local height grid (dem_file.LocalDem) as a NavMap.
+
+Two NavCels per cell with ground, on the nodes of the grid (x east, y north, z its height):
+the cells without ground (cell_valid, e.g. buildings) and those steeper than max_slope_deg are
+left out, as holes, so the NavMap's open edges are the borders of what the robot cannot be on.
+Vectorised: a grid of tens of millions of cells is converted in seconds.
+"""
+
+from array import array
+import math
+from typing import Optional
+
+from navmap_ros_interfaces.msg import NavMap, NavMapSurface
+import numpy as np
+
+from .dem_file import LocalDem
+
+
+def _floats(a) -> array:
+ out = array('f')
+ out.frombytes(np.ascontiguousarray(a, dtype=np.float32).tobytes())
+ return out
+
+
+def _uints(a) -> array:
+ out = array('I')
+ out.frombytes(np.ascontiguousarray(a, dtype=np.uint32).tobytes())
+ return out
+
+
+def ground_cells(dem: LocalDem, max_slope_deg: Optional[float] = None) -> np.ndarray:
+ """(side - 1, side - 1) bool [j, i]: the cells a NavCel is made for."""
+ nc = dem.side - 1
+ ok = np.ones((nc, nc), dtype=bool) if dem.cell_valid is None else dem.cell_valid.astype(bool)
+ if max_slope_deg is not None and max_slope_deg < 90.0:
+ z = dem.z.astype(np.float64)
+ r = dem.resolution
+ # the two triangles of a cell: (a, b, c) and (a, c, d), a = (i, j), b = (i + 1, j),
+ # c = (i + 1, j + 1), d = (i, j + 1); their gradients
+ za, zb, zc, zd = z[:-1, :-1], z[:-1, 1:], z[1:, 1:], z[1:, :-1]
+ g1 = np.hypot((zb - za) / r, (zc - zb) / r)
+ g2 = np.hypot((zc - zd) / r, (zd - za) / r)
+ ok &= np.maximum(g1, g2) <= math.tan(math.radians(max_slope_deg))
+ return ok
+
+
+def navmap_from_dem(dem: LocalDem, max_slope_deg: Optional[float] = None,
+ frame: str = 'map') -> NavMap:
+ """Build the NavMap message of a LocalDem (see the module docstring)."""
+ side = dem.side
+ ok = ground_cells(dem, max_slope_deg)
+ cj, ci = np.nonzero(ok)
+ a = cj * side + ci
+ b = a + 1
+ c = a + side + 1
+ d = a + side
+ # Two triangles per cell, interleaved as navmap_ros::from_occupancy_grid: (a, b, c), (a, c, d)
+ v0 = np.stack([a, a], axis=1).ravel()
+ v1 = np.stack([b, c], axis=1).ravel()
+ v2 = np.stack([c, d], axis=1).ravel()
+ used, inverse = np.unique(np.concatenate([v0, v1, v2]), return_inverse=True)
+ n = len(v0)
+ v0, v1, v2 = inverse[:n], inverse[n:2 * n], inverse[2 * n:]
+ nj, ni = np.divmod(used, side)
+
+ msg = NavMap()
+ msg.header.frame_id = frame
+ msg.positions_x = _floats(dem.x(ni))
+ msg.positions_y = _floats(dem.y(nj))
+ msg.positions_z = _floats(dem.z[nj, ni])
+ if dem.rgb is not None:
+ rgb = dem.rgb[nj, ni]
+ msg.has_vertex_rgba = True
+ msg.colors_r = rgb[:, 0].astype(np.uint8).tobytes()
+ msg.colors_g = rgb[:, 1].astype(np.uint8).tobytes()
+ msg.colors_b = rgb[:, 2].astype(np.uint8).tobytes()
+ msg.colors_a = np.full(len(used), 255, np.uint8).tobytes()
+ else:
+ msg.has_vertex_rgba = False
+ msg.navcels_v0 = _uints(v0)
+ msg.navcels_v1 = _uints(v1)
+ msg.navcels_v2 = _uints(v2)
+ surface = NavMapSurface()
+ surface.frame_id = frame
+ surface.navcels = _uints(np.arange(n))
+ msg.surfaces = [surface]
+ return msg
diff --git a/navmap_tools/test/test_buildings.py b/navmap_tools/test/test_buildings.py
new file mode 100644
index 0000000..34641ff
--- /dev/null
+++ b/navmap_tools/test/test_buildings.py
@@ -0,0 +1,152 @@
+# Copyright 2026 Intelligent Robotics Lab
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+Unit tests for navmap_tools.geo.buildings -- no network involved.
+
+The Catastro and Overpass answers are canned; the footprints become cells of a local grid.
+"""
+
+import json
+
+from navmap_tools.geo import buildings as bmod
+from navmap_tools.geo.buildings import fetch_buildings, footprint_cells, in_spain
+from navmap_tools.geo.cache import DiskCache
+from navmap_tools.geo.dem_file import EnuProjection
+
+import numpy as np
+from pyproj import CRS, Transformer
+
+
+LAT0, LON0 = 40.41690, -3.70350 # the Puerta del Sol
+
+
+def _square(cx, cy, half):
+ return np.array([[cx - half, cy - half], [cx + half, cy - half], [cx + half, cy + half],
+ [cx - half, cy + half]])
+
+
+def test_a_footprint_blocks_the_cells_it_is_in_and_its_patio_stays_ground():
+ side, res = 41, 1.0 # cells of 1 m, centred on the origin, 40 x 40 m
+ ext = _square(0.5, 0.5, 8.0) # x, y in [-7.5, 8.5]
+ patio = _square(0.5, 0.5, 3.0) # x, y in [-2.5, 3.5]
+ cells = footprint_cells([(ext, [patio])], side, res)
+ half = (side - 1) // 2
+
+ def cell(x, y):
+ return cells[int(np.floor(y / res + half)), int(np.floor(x / res + half))]
+
+ assert cell(6.0, 6.0) # inside the building
+ assert not cell(0.5, 0.5) # in the patio
+ assert cell(-7.4, 0.0) # an edge crosses it: blocked (any part of a building)
+ assert cell(8.4, 0.0)
+ assert not cell(-9.0, 0.0) # outside
+ assert not cell(15.0, 15.0)
+ # the patio's edges block their cells too
+ assert cell(-2.4, 0.5) and cell(3.4, 0.5)
+
+
+def test_footprints_beyond_the_grid_are_ignored():
+ cells = footprint_cells([(_square(500.0, 500.0, 5.0), [])], 21, 1.0)
+ assert not cells.any()
+
+
+def test_in_spain():
+ assert in_spain(LAT0, LON0)
+ assert in_spain(28.12, -15.43) # Las Palmas
+ assert not in_spain(48.8562, 2.2976)
+
+
+def _catastro_xml(rings_utm, floors):
+ parts = []
+ for k, (ring, fl) in enumerate(zip(rings_utm, floors)):
+ pos = ' '.join('%.3f %.3f' % (x, y) for x, y in list(ring) + [ring[0]])
+ parts.append(
+ f''
+ f'{fl}'
+ ''
+ f'{pos}'
+ ''
+ '')
+ return ('' +
+ ''.join(parts) + '').encode()
+
+
+def test_catastro_parts_above_ground_in_the_local_frame(monkeypatch, tmp_path):
+ # two parts 10 m east of the origin, one with floors, one underground
+ enu = EnuProjection(LAT0, LON0)
+ to_utm = Transformer.from_crs(CRS.from_epsg(4326), CRS.from_epsg(25830), always_xy=True)
+ lat, lon, _ = enu.to_geodetic(np.array([8.0, 12.0, 12.0, 8.0]),
+ np.array([-2.0, -2.0, 2.0, 2.0]))
+ x, y = to_utm.transform(lon, lat)
+ ring = np.c_[x, y]
+ body = _catastro_xml([ring, ring], [3, 0])
+ calls = []
+
+ def fake_get(cache, url, params, what, timeout=120.0):
+ calls.append(params)
+ return body
+ monkeypatch.setattr(bmod, '_cached_get', fake_get)
+ footprints, source = fetch_buildings('auto', LAT0, LON0, 50.0, DiskCache(tmp_path))
+ assert source == 'Catastro'
+ assert len(calls) == 1 # a small area: one tile
+ assert len(footprints) == 1 # the underground part is left out
+ ext, patios = footprints[0]
+ assert patios == []
+ assert np.allclose(ext.min(axis=0), [8.0, -2.0], atol=0.01)
+ assert np.allclose(ext.max(axis=0), [12.0, 2.0], atol=0.01)
+
+
+def test_osm_buildings_where_the_catastro_has_none(monkeypatch, tmp_path):
+ no_records = (b'No records')
+ monkeypatch.setattr(bmod, '_cached_get', lambda *a, **k: no_records)
+ enu = EnuProjection(LAT0, LON0)
+ lat, lon, _ = enu.to_geodetic(np.array([0.0, 6.0, 6.0, 0.0, 0.0]),
+ np.array([0.0, 0.0, 6.0, 6.0, 0.0]))
+ way = {'type': 'way', 'id': 1, 'tags': {'building': 'yes'},
+ 'geometry': [{'lat': float(a), 'lon': float(o)} for a, o in zip(lat, lon)]}
+ roof = dict(way, id=2, tags={'building': 'roof'})
+
+ class Answer:
+ status_code = 200
+ content = json.dumps({'elements': [way, roof]}).encode()
+ monkeypatch.setattr(bmod.requests, 'post', lambda *a, **k: Answer())
+ footprints, source = fetch_buildings('auto', LAT0, LON0, 50.0, DiskCache(tmp_path))
+ assert source == 'OpenStreetMap'
+ assert len(footprints) == 1 # building=roof is no obstacle at ground level
+ assert np.allclose(footprints[0][0].max(axis=0), [6.0, 6.0], atol=0.01)
+
+
+def test_where_the_parts_fail_the_whole_buildings_are_asked(monkeypatch, tmp_path):
+ enu = EnuProjection(LAT0, LON0)
+ to_utm = Transformer.from_crs(CRS.from_epsg(4326), CRS.from_epsg(25830), always_xy=True)
+ lat, lon, _ = enu.to_geodetic(np.array([8.0, 12.0, 12.0, 8.0]),
+ np.array([-2.0, -2.0, 2.0, 2.0]))
+ x, y = to_utm.transform(lon, lat)
+ building = _catastro_xml([np.c_[x, y]], [0]).replace(b'BuildingPart', b'Building')
+ asked = []
+
+ def fake_get(cache, url, params, what, timeout=120.0):
+ asked.append(params['typenames'])
+ if params['typenames'] == 'BU:BuildingPart':
+ raise RuntimeError('HTTP 404')
+ return building
+ monkeypatch.setattr(bmod, '_cached_get', fake_get)
+ footprints, source = fetch_buildings('catastro', LAT0, LON0, 50.0, DiskCache(tmp_path))
+ assert source == 'Catastro'
+ assert 'BU:Building' in asked
+ assert len(footprints) == 1 # a whole building: no floors, still a footprint
diff --git a/navmap_tools/test/test_cli.py b/navmap_tools/test/test_cli.py
index eb65b50..92f8041 100644
--- a/navmap_tools/test/test_cli.py
+++ b/navmap_tools/test/test_cli.py
@@ -608,3 +608,34 @@ def counting(path, cell):
cli_mod._load_dem(args, None, None) # the world's
cli_mod._dem_geometry(args) # the DEM's
assert len(calls) == 1
+
+
+def test_navmap_out_with_buildings_leaves_their_cells_out(tmp_path, monkeypatch):
+ import numpy as np
+
+ from navmap_tools import cli as cli_mod
+ from navmap_tools.cli import run
+ from navmap_tools.resample import read_navmap
+
+ def fake_buildings(source, lat, lon, radius, cache):
+ assert source == 'osm'
+ # edges inside cells (x, y in [-4.25, 4.25]): cells -5 .. 4 on each axis
+ square = np.array([[-4.25, -4.25], [4.25, -4.25], [4.25, 4.25], [-4.25, 4.25]])
+ return [(square, [])], 'OpenStreetMap'
+ monkeypatch.setattr(cli_mod, 'fetch_buildings', fake_buildings)
+ out = tmp_path / 'map.navmap'
+ args = _BASE + ['--dem-source', 'synthetic', '--resolution', '1', '--max-slope-deg', '89',
+ '--cache-dir', str(tmp_path / 'c'), '--navmap-out', str(out)]
+ assert run(args) == 0
+ full = len(read_navmap(str(out)).navcels_v0)
+ assert full == 2 * 40 * 40
+ assert run(args + ['--buildings', 'osm']) == 0
+ # the building blocks every cell any part of it is in: 10 x 10
+ assert len(read_navmap(str(out)).navcels_v0) == full - 2 * 10 * 10
+
+
+def test_buildings_need_a_grid_output():
+ from navmap_tools.cli import run
+
+ with pytest.raises(SystemExit):
+ run(_BASE + ['--gazebo', '--buildings', 'osm'])
diff --git a/navmap_tools/test/test_dem_navmap.py b/navmap_tools/test/test_dem_navmap.py
new file mode 100644
index 0000000..3b2f638
--- /dev/null
+++ b/navmap_tools/test/test_dem_navmap.py
@@ -0,0 +1,73 @@
+# Copyright 2026 Intelligent Robotics Lab
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Unit tests for navmap_tools.geo.dem_navmap: a local height grid as a NavMap."""
+
+import math
+
+from navmap_tools.geo.dem_file import LocalDem
+from navmap_tools.geo.dem_navmap import navmap_from_dem
+from navmap_tools.resample import read_navmap, write_navmap
+
+import numpy as np
+
+
+def _dem(side=5, res=1.0, z=None, cell_valid=None, rgb=None):
+ return LocalDem(
+ side=side, resolution=res, data_resolution=res, radius=(side - 1) / 2 * res,
+ origin_lat=40.0, origin_lon=-3.0, reference_altitude=0.0,
+ requested_reference=float('nan'), source=0,
+ z=np.zeros((side, side), np.float32) if z is None else z, rgb=rgb,
+ cell_valid=cell_valid)
+
+
+def test_two_navcels_per_cell_on_the_nodes():
+ msg = navmap_from_dem(_dem())
+ assert len(msg.navcels_v0) == 2 * 16
+ assert len(msg.positions_x) == 25
+ assert min(msg.positions_x) == -2.0 and max(msg.positions_y) == 2.0
+ assert len(msg.surfaces) == 1 and len(msg.surfaces[0].navcels) == 32
+
+
+def test_cells_without_ground_are_holes_and_unused_vertices_go():
+ cv = np.ones((4, 4), np.uint8)
+ cv[1:3, 1:3] = 0 # the four central cells: a building
+ msg = navmap_from_dem(_dem(cell_valid=cv))
+ assert len(msg.navcels_v0) == 2 * 12
+ assert len(msg.positions_x) == 24 # only the centre node is left without a NavCel
+ cv[:] = 0
+ cv[0, 0] = 1
+ msg = navmap_from_dem(_dem(cell_valid=cv))
+ assert len(msg.navcels_v0) == 2 and len(msg.positions_x) == 4
+
+
+def test_steep_cells_are_left_out():
+ z = np.zeros((5, 5), np.float32)
+ z[:, 4] = 5.0 # the last column of cells rises 5 m in 1 m
+ msg = navmap_from_dem(_dem(z=z), max_slope_deg=30.0)
+ assert len(msg.navcels_v0) == 2 * 12
+ msg = navmap_from_dem(_dem(z=z), max_slope_deg=89.0)
+ assert len(msg.navcels_v0) == 2 * 16
+ assert math.isclose(max(msg.positions_z), 5.0)
+
+
+def test_colours_and_a_round_trip_through_a_file(tmp_path):
+ rgb = np.zeros((5, 5, 3), np.uint8)
+ rgb[..., 0] = 200
+ msg = navmap_from_dem(_dem(rgb=rgb))
+ assert msg.has_vertex_rgba and bytes(msg.colors_r)[0] == 200
+ path = str(tmp_path / 'm.navmap')
+ write_navmap(msg, path)
+ back = read_navmap(path)
+ assert len(back.navcels_v0) == 32 and len(back.positions_x) == 25