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