From 45bd8289af8246812c5a10c41cfa649cbbbf6b70 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 17 Nov 2026 13:50:15 +0100 Subject: [PATCH 1/2] 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 From a2c2beb69355f93edde526c9262048f479964831 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Sun, 22 Nov 2026 01:06:36 +0100 Subject: [PATCH 2/2] navmap_tools: --navmap-keep, only the part of the NavMap connected to a point (or the largest) A NavMap made from a terrain mesh with its buildings keeps their flat roofs as islands, cut off by the walls the slope limit leaves out; closed patios are islands too. --navmap-keep X,Y keeps the NavCels connected to that point (the nearest cell with ground), --navmap-keep largest the largest part. The parts are found on the grid by a vectorised union-find (16 M cells in about 2 s). --- README.md | 6 +- navmap_tools/navmap_tools/cli.py | 43 +++++++- navmap_tools/navmap_tools/geo/dem_navmap.py | 76 +++++++++++++- navmap_tools/test/test_cli.py | 57 +++++++++++ navmap_tools/test/test_dem_navmap.py | 108 +++++++++++++++++++- 5 files changed, 280 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index f003073..e9e9325 100644 --- a/README.md +++ b/README.md @@ -349,12 +349,12 @@ 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. +`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. `--navmap-keep X,Y` keeps only the NavCels connected to that point (metres in the local frame of `--center`, e.g. where the robot starts), and `--navmap-keep largest` the largest connected part: what the robot can reach, without the islands of the grid (the roofs of buildings when the terrain comes from a mesh with them, closed patios). ```bash -# 2 x 2 km around the Puerta del Sol (Madrid), cells of 0.5 m +# 2 x 2 km around the Puerta del Sol (Madrid), cells of 0.5 m, only what is reachable from the centre 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 + --dem-source ign --buildings auto --navmap-out sol.navmap --navmap-keep 0,0 ``` --- diff --git a/navmap_tools/navmap_tools/cli.py b/navmap_tools/navmap_tools/cli.py index 3d17798..696f3e7 100644 --- a/navmap_tools/navmap_tools/cli.py +++ b/navmap_tools/navmap_tools/cli.py @@ -40,7 +40,7 @@ add_imagery, build_local_dem, DemSource, read_dem_file, SyntheticDem, SyntheticImagery, write_dem_file, ) -from .geo.dem_navmap import navmap_from_dem +from .geo.dem_navmap import ground_cells, 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, @@ -105,6 +105,22 @@ def _parse_center(value: str): return lat, lon +def _parse_keep(value: str): + """--navmap-keep: 'largest', or 'X,Y' as two finite floats.""" + if value.strip() == 'largest': + return 'largest' + parts = value.split(',') + try: + if len(parts) != 2: + raise ValueError + x, y = (float(v) for v in parts) + except ValueError: + raise ValueError(f'--navmap-keep must be X,Y (metres) or largest, not {value!r}') from None + if not (math.isfinite(x) and math.isfinite(y)): + raise ValueError(f'--navmap-keep must be finite, not {value!r}') + return (x, y) + + def build_arg_parser() -> argparse.ArgumentParser: """Build the argparse parser for navmap_gis_tool.""" parser = argparse.ArgumentParser( @@ -218,6 +234,14 @@ def build_arg_parser() -> argparse.ArgumentParser: 'than --max-slope-deg; vertex colours with --dem-imagery. Vectorised, for large areas ' '(unlike --navmap)' )) + parser.add_argument( + '--navmap-keep', default=None, metavar='X,Y|largest', + help=( + '--navmap-out: keep only the NavCels connected to the point X,Y (metres in the local ' + 'frame of --center, e.g. where the robot starts; the nearest cell with ground is ' + 'taken), or to the largest connected part: what the robot can reach, without roofs ' + 'and closed patios (default: every cell with ground)' + )) parser.add_argument( '--mesh-world-elevation', type=float, default=None, metavar='METERS', help='--dem-source mesh: altitude above sea level of the mesh z = 0 (default: 0)') @@ -294,6 +318,13 @@ def run(argv=None) -> int: parser.error('--dem-source mesh needs --mesh-file') 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.navmap_keep is not None: + if not args.navmap_out: + parser.error('--navmap-keep needs --navmap-out') + try: + args.navmap_keep = _parse_keep(args.navmap_keep) + except ValueError as e: + parser.error(str(e)) if args.dem_source == 'mesh' and args.mesh_cell <= 0: parser.error('--mesh-cell must be positive') if args.dem_data_resolution < 0: @@ -665,10 +696,16 @@ 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) + keep = getattr(args, 'navmap_keep', None) + msg = navmap_from_dem(grid, args.max_slope_deg, keep=keep) write_navmap(msg, args.navmap_out) + kept = '' + if keep is not None: + total = 2 * int(ground_cells(grid, args.max_slope_deg).sum()) + where = 'the largest part' if keep == 'largest' else 'the part at %g, %g' % keep + kept = f'; {where}: {total - len(msg.navcels_v0)} of {total} left out, not connected' print(f'[navmap_gis_tool] wrote {args.navmap_out} ({len(msg.navcels_v0)} NavCels, ' - f'{len(msg.positions_x)} vertices)') + f'{len(msg.positions_x)} vertices{kept})') def _write_dem(args, dem, imagery, lat, lon, cache=None) -> None: diff --git a/navmap_tools/navmap_tools/geo/dem_navmap.py b/navmap_tools/navmap_tools/geo/dem_navmap.py index ad08333..629926f 100644 --- a/navmap_tools/navmap_tools/geo/dem_navmap.py +++ b/navmap_tools/navmap_tools/geo/dem_navmap.py @@ -18,12 +18,14 @@ 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. +Optionally only the part connected to a point (or the largest part) is kept: what the robot can +reach, without the roofs and the closed patios that are islands of the grid. Vectorised: a grid of tens of millions of cells is converted in seconds. """ from array import array import math -from typing import Optional +from typing import Optional, Tuple, Union from navmap_ros_interfaces.msg import NavMap, NavMapSurface import numpy as np @@ -59,11 +61,79 @@ def ground_cells(dem: LocalDem, max_slope_deg: Optional[float] = None) -> np.nda return ok +def connected_parts(ok: np.ndarray) -> np.ndarray: + """ + Label the connected parts of the cells of `ok` (2D bool): an int per cell, -1 where False. + + Cells that share an edge have the same label, and so do their NavCels, which share edges only + across those (two cells that touch at a corner share a vertex). + + Union-find over the edges between neighbouring cells, every edge at once: each round hooks the + larger root of the two ends of an edge onto the smaller one and compresses the paths, until + both ends of every edge have the same root. + """ + idx = np.full(ok.shape, -1, np.int64) + cells = np.flatnonzero(ok) + idx.flat[cells] = np.arange(len(cells)) + right = ok[:, :-1] & ok[:, 1:] + up = ok[:-1, :] & ok[1:, :] + a = np.concatenate([idx[:, :-1][right], idx[:-1, :][up]]) + b = np.concatenate([idx[:, 1:][right], idx[1:, :][up]]) + parent = np.arange(len(cells)) + while len(a): + pa, pb = parent[a], parent[b] + differ = pa != pb + if not differ.any(): + break + a, b, pa, pb = a[differ], b[differ], pa[differ], pb[differ] + np.minimum.at(parent, np.maximum(pa, pb), np.minimum(pa, pb)) + while True: + grand = parent[parent] + if np.array_equal(grand, parent): + break + parent = grand + labels = np.full(ok.shape, -1, np.int64) + labels.flat[cells] = parent + return labels + + +def keep_connected(dem: LocalDem, ok: np.ndarray, + keep: Union[str, Tuple[float, float]]) -> np.ndarray: + """ + Return `ok` without the cells that are not connected to `keep`. + + `keep`: a point (x, y) in the local frame, whose nearest cell of `ok` is taken (it may fall on + a hole), or 'largest', the part of most cells. + """ + labels = connected_parts(ok) + if not ok.any(): + return ok + if isinstance(keep, str): + if keep != 'largest': + raise ValueError("keep must be 'largest' or (x, y), not %r" % keep) + counts = np.bincount(labels[ok]) + return labels == int(np.argmax(counts)) + x, y = (float(v) for v in keep) + cj, ci = np.nonzero(ok) + half = dem.resolution / 2.0 + d2 = (dem.x(ci) + half - x) ** 2 + (dem.y(cj) + half - y) ** 2 + k = int(np.argmin(d2)) + return labels == labels[cj[k], ci[k]] + + 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).""" + frame: str = 'map', + keep: Optional[Union[str, Tuple[float, float]]] = None) -> NavMap: + """ + Build the NavMap message of a LocalDem (see the module docstring). + + `keep`: None, every cell with ground; (x, y), only the part connected to that point (see + keep_connected); 'largest', the largest part. + """ side = dem.side ok = ground_cells(dem, max_slope_deg) + if keep is not None: + ok = keep_connected(dem, ok, keep) cj, ci = np.nonzero(ok) a = cj * side + ci b = a + 1 diff --git a/navmap_tools/test/test_cli.py b/navmap_tools/test/test_cli.py index 92f8041..46966e8 100644 --- a/navmap_tools/test/test_cli.py +++ b/navmap_tools/test/test_cli.py @@ -25,6 +25,7 @@ from navmap_tools.cli import ( _default_package_name, _parse_center, + _parse_keep, _texture_pixels_for, build_arg_parser, ) @@ -413,6 +414,62 @@ def write(path, quads): assert read_dem_file(out).cell_valid is None +@pytest.mark.parametrize('value, expected', [ + ('largest', 'largest'), ('1.5,-2', (1.5, -2.0)), (' 0 , 0 ', (0.0, 0.0))]) +def test_parse_keep(value, expected): + assert _parse_keep(value) == expected + + +@pytest.mark.parametrize('value', ['', 'biggest', '1', '1,2,3', 'a,b', 'nan,0', '0,inf']) +def test_parse_keep_rejects(value): + with pytest.raises(ValueError): + _parse_keep(value) + + +def test_navmap_keep_leaves_out_the_roofs_of_a_mesh_with_buildings(tmp_path, capsys): + import struct + + from navmap_tools.cli import run + from navmap_tools.resample import read_navmap + + def write(path, quads): + tris = [] + for x0, y0, x1, y1, z in quads: + tris += [(x0, y0, z, x1, y0, z, x1, y1, z), (x0, y0, z, x1, y1, z, x0, y1, z)] + with open(path, 'wb') as f: + f.write(b'\0' * 80 + struct.pack('= 0)): + out.setdefault(labels[j, i], set()).add((j, i)) + return {frozenset(v) for v in out.values()} + + +def test_connected_parts_of_a_spiral_and_of_random_grids(): + # concentric rings, each joined to the next by one cell: a corridor 20 rings long, the worst + # case of a label that has to travel far + n = 41 + ok = np.zeros((n, n), bool) + lo, hi = 0, n - 1 + while lo < hi: + ok[lo, lo:hi + 1] = ok[lo:hi + 1, hi] = ok[hi, lo:hi + 1] = True + ok[lo + 2:hi + 1, lo] = True # the ring is open between (lo, lo) and (lo + 2, lo) + if lo + 2 < hi: + ok[lo + 2, lo + 1] = True # the cell that joins it to the next ring + lo += 2 + hi -= 2 + assert len(_flood_fill_parts(ok)) == 1 + assert len(np.unique(connected_parts(ok)[ok])) == 1 + rng = np.random.default_rng(1) + for density in (0.45, 0.6, 0.75): # around the percolation threshold: parts of every size + ok = rng.random((120, 150)) < density + assert _parts(connected_parts(ok)) == _flood_fill_parts(ok) + + +def _roofs_dem(): + """ + Make 9 x 9 cells of ground, with a building and a closed patio. + + The walls of the building (its cells 0 and 4) are too steep, so its flat roof (cells 1..3, + 3 x 3) is an island; the patio is 1 cell inside a ring of holes. + """ + z = np.zeros((10, 10), np.float32) + z[1:5, 1:5] = 10.0 # the nodes of the roof + cv = np.ones((9, 9), np.uint8) + cv[5:8, 5:8] = 0 + cv[6, 6] = 1 # the patio + return _dem(side=10, z=z, cell_valid=cv) + + +def test_navmap_keeps_only_the_part_connected_to_a_point(): + dem = _roofs_dem() + every = navmap_from_dem(dem, max_slope_deg=30.0) + at_origin = navmap_from_dem(dem, max_slope_deg=30.0, keep=(4.0, -4.0)) + # the roof and the patio are left out + assert len(every.navcels_v0) - len(at_origin.navcels_v0) == 2 * (9 + 1) + assert max(at_origin.positions_z) == 0.0 + on_the_roof = navmap_from_dem(dem, max_slope_deg=30.0, keep=(-2.0, -2.0)) + assert len(on_the_roof.navcels_v0) == 2 * 9 + assert min(on_the_roof.positions_z) == 10.0 + + +def test_a_point_on_a_hole_takes_the_nearest_cell_and_largest_takes_the_most_cells(): + dem = _roofs_dem() + ok = np.ones((9, 9), bool) + ok[5:8, 5:8] = False + ok[6, 6] = True + patio = keep_connected(dem, ok, (2.5, 2.5)) # the centre of the patio's cell (6, 6) + assert patio.sum() == 1 and patio[6, 6] + near = keep_connected(dem, ok, (1.4, 1.4)) # on the hole at cell (5, 5): nearest is ground + assert near.sum() == 81 - 9 + assert keep_connected(dem, ok, 'largest').sum() == 81 - 9 + with pytest.raises(ValueError): + keep_connected(dem, ok, 'biggest') + assert not keep_connected(dem, np.zeros((9, 9), bool), 'largest').any()