From 45bd8289af8246812c5a10c41cfa649cbbbf6b70 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 17 Nov 2026 13:50:15 +0100 Subject: [PATCH] navmap_tools: buildings from the Catastro / OpenStreetMap, and large NavMaps of a place navmap_gis_tool: - --buildings {auto,catastro,osm}: downloads the footprints of the buildings (the Catastro INSPIRE WFS in Spain, by tiles, retried with backoff, split in four when the service keeps failing and, where it still fails on the building parts, asked for the whole buildings; OpenStreetMap through Overpass elsewhere or where the Catastro has none) and leaves without ground every cell any part of a building is in; patios stay ground. Answers are cached on disk. - --navmap-out FILE: the local height grid as a NavMap, two NavCels per cell with ground, without the cells that have none (--buildings, --dem-holes-mesh) or are steeper than --max-slope-deg; vertex colours with --dem-imagery. Vectorised (tens of millions of cells), unlike --navmap, whose sampling loop is pure Python. - --dem-holes-mesh now also works with --navmap-out. --- README.md | 12 + navmap_tools/navmap_tools/cli.py | 119 +++++--- navmap_tools/navmap_tools/geo/buildings.py | 309 ++++++++++++++++++++ navmap_tools/navmap_tools/geo/dem_navmap.py | 102 +++++++ navmap_tools/test/test_buildings.py | 152 ++++++++++ navmap_tools/test/test_cli.py | 31 ++ navmap_tools/test/test_dem_navmap.py | 73 +++++ 7 files changed, 764 insertions(+), 34 deletions(-) create mode 100644 navmap_tools/navmap_tools/geo/buildings.py create mode 100644 navmap_tools/navmap_tools/geo/dem_navmap.py create mode 100644 navmap_tools/test/test_buildings.py create mode 100644 navmap_tools/test/test_dem_navmap.py 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