diff --git a/mavros/launch/apm.launch b/mavros/launch/apm.launch index 49c6f57bd..25f2e5ae2 100644 --- a/mavros/launch/apm.launch +++ b/mavros/launch/apm.launch @@ -10,6 +10,9 @@ + + + @@ -21,4 +24,13 @@ + + + + + + + + + diff --git a/mavros/mavros_plugins.xml b/mavros/mavros_plugins.xml index 3efbed0ad..82b13b1e5 100644 --- a/mavros/mavros_plugins.xml +++ b/mavros/mavros_plugins.xml @@ -140,4 +140,4 @@ Required by all plugins. @plugin wind_estimation - + diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index f9ea5dcb9..4137ded85 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -18,6 +18,7 @@ set(CMAKE_C_EXTENSIONS ON) set(CMAKE_CXX_EXTENSIONS ON) find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) # find mavros dependencies find_package(rclcpp REQUIRED) @@ -141,7 +142,7 @@ add_library(mavros_extras_plugins SHARED src/plugins/vision_pose_estimate.cpp src/plugins/vision_speed_estimate.cpp src/plugins/wheel_odometry.cpp - # [[[end]]] (sum: E3zv4+Yl1z) + # [[[end]]] (sum: WzAyzLqsp3) ) target_link_libraries(mavros_extras_plugins PUBLIC ${geographic_msgs_TARGETS} @@ -216,9 +217,19 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) +# Python terrain server library + entry-point +ament_python_install_package(${PROJECT_NAME} + SETUP_CFG setup.cfg +) +install(PROGRAMS + scripts/terrain_tile_server + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_gmock REQUIRED) + find_package(ament_cmake_pytest REQUIRED) find_package(ament_lint_auto REQUIRED) @@ -231,6 +242,8 @@ if(BUILD_TESTING) endif() ament_lint_auto_find_test_dependencies() + + ament_add_pytest_test(test_srtm test/test_srtm.py) endif() #ament_export_dependencies(console_bridge) diff --git a/mavros_extras/mavros_extras/__init__.py b/mavros_extras/mavros_extras/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mavros_extras/mavros_extras/srtm.py b/mavros_extras/mavros_extras/srtm.py new file mode 100644 index 000000000..8f2bcf5ef --- /dev/null +++ b/mavros_extras/mavros_extras/srtm.py @@ -0,0 +1,396 @@ +""" +Manage SRTM tiles: download, cache, parse, and look up elevation. + +Handles .hgt files from the Shuttle Radar Topography Mission dataset. +Supports SRTM1 (1 arc-second, 3601x3601) and SRTM3 (3 arc-second, 1201x1201). + +Written by Zeke Sarosi +Inspired by the Pymavlink implementation +""" + +from __future__ import annotations + +import array +from collections import OrderedDict +import io +import logging +import math +import os +from pathlib import Path +import struct +import threading +import urllib.error +import urllib.request +import zipfile + +logger = logging.getLogger(__name__) + +SRTM1_SIDE = 3601 +SRTM3_SIDE = 1201 +SRTM_VOID = -32768 + +RADIUS_OF_EARTH = 6378100.0 + +# MAVLink TERRAIN_REQUEST grid layout (common.xml §TERRAIN_REQUEST): +# The terrain around a position is divided into a GRID_ROWS × GRID_COLS +# array of blocks. Each block is TILE_DIM × TILE_DIM elevation samples. +GRID_COLS = 8 +GRID_ROWS = 7 +TILE_DIM = 4 + +_CONTINENTS = ( + 'Africa/', + 'Australia/', + 'Eurasia/', + 'Islands/', + 'North_America/', + 'South_America/', +) + +try: + from mavros_extras.srtm_continent_map import lookup_continent as _lookup_continent +except ImportError: + _lookup_continent = None + + +class SrtmTile: + """Single loaded SRTM .hgt tile with compact int16 storage.""" + + __slots__ = ('data', 'side') + + def __init__(self, data: array.array, side: int): + self.data = data + self.side = side + + def __repr__(self) -> str: + return f'SrtmTile(side={self.side})' + + +class SrtmManager: + """Thread-safe SRTM tile manager with LRU cache and optional auto-download.""" + + def __init__( + self, + terrain_data_path: str = '', + auto_download: bool = False, + download_host: str = 'terrain.ardupilot.org', + srtm_source: str = 'SRTM3', + max_cache_tiles: int = 64, + ): + self._terrain_data_path = terrain_data_path + self._auto_download = auto_download + self._download_host = download_host + self._srtm_source = srtm_source + self._max_cache_tiles = max_cache_tiles + + self._cache: OrderedDict[int, SrtmTile | None] = OrderedDict() + self._file_index: dict[str, Path] = {} + self._download_failed: set[int] = set() + self._lock = threading.Lock() + + if not self._terrain_data_path and self._auto_download: + home = os.environ.get('HOME', '/tmp') + self._terrain_data_path = os.path.join( + home, '.cache', 'mavros', 'terrain', self._srtm_source + ) + logger.info('Auto-download cache: %s', self._terrain_data_path) + + if self._terrain_data_path: + Path(self._terrain_data_path).mkdir(parents=True, exist_ok=True) + self._build_file_index() + + def _build_file_index(self) -> None: + root = Path(self._terrain_data_path) + if not root.exists(): + return + count = 0 + for hgt in root.rglob('*.hgt'): + self._file_index[hgt.name] = hgt + count += 1 + logger.info('Indexed %d .hgt files in %s', count, self._terrain_data_path) + + # ------------------------------------------------------------------ keys + + @staticmethod + def _tile_key(lat: int, lon: int) -> int: + """Return a unique integer key for a 1-degree tile corner.""" + return (lat + 90) * 360 + (lon + 180) + + @staticmethod + def _tile_filename(lat: int, lon: int) -> str: + """Return the standard .hgt filename for a tile (e.g. N47E011.hgt).""" + ns = 'N' if lat >= 0 else 'S' + ew = 'E' if lon >= 0 else 'W' + return f'{ns}{abs(lat):02d}{ew}{abs(lon):03d}.hgt' + + # ------------------------------------------------------------------ load + + def _load_tile(self, lat: int, lon: int) -> SrtmTile | None: + if not self._terrain_data_path: + return None + + filename = self._tile_filename(lat, lon) + filepath = self._file_index.get(filename) + + if filepath is None: + filepath = Path(self._terrain_data_path) / filename + if not filepath.exists(): + return None + self._file_index[filename] = filepath + + file_size = filepath.stat().st_size + expected_1 = SRTM1_SIDE * SRTM1_SIDE * 2 + expected_3 = SRTM3_SIDE * SRTM3_SIDE * 2 + + if file_size == expected_1: + side = SRTM1_SIDE + elif file_size == expected_3: + side = SRTM3_SIDE + else: + logger.warning( + 'Unexpected file size for %s: %d bytes (expected %d or %d)', + filename, + file_size, + expected_3, + expected_1, + ) + return None + + raw = filepath.read_bytes() + n = side * side + data = array.array('h', struct.unpack(f'>{n}h', raw)) + + logger.info('Loaded tile %s (%d×%d)', filename, side, side) + return SrtmTile(data, side) + + # ------------------------------------------------------------------ download + + def _download_tile(self, lat: int, lon: int) -> bool: + """ + Download a tile zip from the ArduPilot SRTM mirror. + + Use the continent lookup table for a direct download when available, + falling back to trying all continent directories sequentially. + Only the expected .hgt file is extracted to prevent zip-slip attacks. + """ + filename = self._tile_filename(lat, lon) + hgt_path = Path(self._terrain_data_path) / filename + if hgt_path.exists(): + return True + + zip_name = filename + '.zip' + base_url = f'https://{self._download_host}/{self._srtm_source}' + + continents: tuple[str, ...] | list[str] + if _lookup_continent is not None: + known = _lookup_continent(lat, lon) + if known is None: + logger.info('No SRTM coverage for %s (continent map)', filename) + return False + continents = (known,) + else: + continents = _CONTINENTS + + zip_bytes: bytes | None = None + for continent in continents: + url = f'{base_url}/{continent}{zip_name}' + if not url.startswith(('https://', 'http://')): + raise ValueError(f'Refusing non-HTTP URL: {url}') + try: + logger.info('Downloading %s', url) + # URL scheme is validated above; host is a trusted configuration parameter. + with urllib.request.urlopen( # nosemgrep: dynamic-urllib-use-detected + urllib.request.Request(url), timeout=60 + ) as resp: + zip_bytes = resp.read() + break + except urllib.error.URLError: + continue + + if zip_bytes is None: + logger.warning('Tile %s not found on server', filename) + return False + + try: + with zipfile.ZipFile(io.BytesIO(zip_bytes)) as zf: + if filename not in zf.namelist(): + logger.warning('Archive for %s does not contain the expected .hgt', filename) + return False + zf.extract(filename, self._terrain_data_path) + except zipfile.BadZipFile: + logger.error('Corrupt zip for %s', filename) + return False + + if hgt_path.exists(): + logger.info('Downloaded: %s', filename) + with self._lock: + self._file_index[filename] = hgt_path + return True + + logger.warning('Extraction produced no .hgt for %s', filename) + return False + + # ------------------------------------------------------------------ cache + + def _get_tile(self, lat: int, lon: int) -> SrtmTile | None: + """Return a tile from cache, loading or downloading as needed.""" + key = self._tile_key(lat, lon) + + with self._lock: + if key in self._cache: + self._cache.move_to_end(key) + return self._cache[key] + + tile = self._load_tile(lat, lon) + + if tile is None and self._auto_download: + if key not in self._download_failed: + if self._download_tile(lat, lon): + tile = self._load_tile(lat, lon) + else: + self._download_failed.add(key) + + with self._lock: + if key in self._cache: + self._cache.move_to_end(key) + return self._cache[key] + self._cache[key] = tile + while len(self._cache) > self._max_cache_tiles: + self._cache.popitem(last=False) + + return tile + + # ------------------------------------------------------------------ elevation + + def lookup_elevation(self, lat_deg: float, lon_deg: float) -> float | None: + """ + Interpolate elevation at a WGS-84 coordinate using bilinear weights. + + When one or more of the four surrounding grid cells are void, + the available corners are averaged with renormalized bilinear weights + instead of falling back to a single arbitrary sample. + """ + tlat = math.floor(lat_deg) + tlon = math.floor(lon_deg) + + tile = self._get_tile(tlat, tlon) + if tile is None: + return None + + frac_lat = lat_deg - tlat + frac_lon = lon_deg - tlon + + row_f = (1.0 - frac_lat) * (tile.side - 1) + col_f = frac_lon * (tile.side - 1) + + r0 = max(0, min(int(math.floor(row_f)), tile.side - 2)) + c0 = max(0, min(int(math.floor(col_f)), tile.side - 2)) + + fr = row_f - r0 + fc = col_f - c0 + + s = tile.side + v00 = tile.data[r0 * s + c0] + v01 = tile.data[r0 * s + c0 + 1] + v10 = tile.data[(r0 + 1) * s + c0] + v11 = tile.data[(r0 + 1) * s + c0 + 1] + + corners = ( + (v00, (1 - fc) * (1 - fr)), + (v01, fc * (1 - fr)), + (v10, (1 - fc) * fr), + (v11, fc * fr), + ) + valid = [(float(v), w) for v, w in corners if v != SRTM_VOID] + + if not valid: + return None + if len(valid) == 4: + return sum(v * w for v, w in valid) + + total_w = sum(w for _, w in valid) + return sum(v * w for v, w in valid) / total_w + + +# ---------------------------------------------------------------------- geodesic + + +def gps_newpos( + lat_deg: float, lon_deg: float, bearing_deg: float, distance_m: float +) -> tuple[float, float]: + """ + Compute a new position along a rhumb line. + + Match MAVProxy ``mp_util.gps_newpos`` for consistency with ArduPilot. + """ + if distance_m == 0.0: + return (lat_deg, lon_deg) + + lat1 = max(-math.pi / 2 + 1e-15, min(math.pi / 2 - 1e-15, math.radians(lat_deg))) + lon1 = math.radians(lon_deg) + tc = -math.radians(bearing_deg) + d = distance_m / RADIUS_OF_EARTH + + lat = lat1 + d * math.cos(tc) + lat = max(-math.pi / 2 + 1e-15, min(math.pi / 2 - 1e-15, lat)) + + if abs(lat - lat1) < 1e-15: + q = math.cos(lat1) + else: + dphi = math.log(math.tan(lat / 2 + math.pi / 4) / math.tan(lat1 / 2 + math.pi / 4)) + q = (lat - lat1) / dphi + + dlon = -d * math.sin(tc) / q + lon = math.fmod(lon1 + dlon + math.pi, 2 * math.pi) - math.pi + + return (math.degrees(lat), math.degrees(lon)) + + +def gps_offset( + lat_deg: float, lon_deg: float, east_m: float, north_m: float +) -> tuple[float, float]: + """Offset a position by east/north meters.""" + bearing = math.degrees(math.atan2(east_m, north_m)) + distance = math.hypot(east_m, north_m) + return gps_newpos(lat_deg, lon_deg, bearing, distance) + + +# ---------------------------------------------------------------------- grid protocol + + +def compute_terrain_data_block( + mgr: SrtmManager, + lat_e7: int, + lon_e7: int, + grid_spacing: int, + bit: int, +) -> list[int] | None: + """ + Compute the 16 elevation values for one 4x4 terrain block. + + Parameter names match the MAVLink TERRAIN_DATA message fields. + Return a list of 16 int16 elevations (row-major within the block), + or None if any sample is unavailable. + """ + base_lat = lat_e7 / 1e7 + base_lon = lon_e7 / 1e7 + spacing = float(grid_spacing) + bit_spacing = spacing * TILE_DIM + + col = bit % GRID_COLS + row = bit // GRID_COLS + + tile_lat, tile_lon = gps_offset(base_lat, base_lon, bit_spacing * col, bit_spacing * row) + + data: list[int] = [] + for i in range(TILE_DIM * TILE_DIM): + y = i % TILE_DIM + x = i // TILE_DIM + + pt_lat, pt_lon = gps_offset(tile_lat, tile_lon, spacing * y, spacing * x) + elev = mgr.lookup_elevation(pt_lat, pt_lon) + if elev is None: + return None + data.append(int(round(elev))) + + return data diff --git a/mavros_extras/mavros_extras/srtm_continent_map.py b/mavros_extras/mavros_extras/srtm_continent_map.py new file mode 100644 index 000000000..486cb0d26 --- /dev/null +++ b/mavros_extras/mavros_extras/srtm_continent_map.py @@ -0,0 +1,96 @@ +""" +Auto-generated SRTM continent lookup table. + +Maps each 1-degree tile to its continent directory on terrain.ardupilot.org. +Generated by ../../tools/generate_srtm_continent_map.py -- DO NOT EDIT. +""" + +from __future__ import annotations + +import zlib + +_CONTINENTS = ( + 'Africa/', + 'Australia/', + 'Eurasia/', + 'Islands/', + 'North_America/', + 'South_America/', +) + +_LAT_MIN = -56 +_LAT_RANGE = 116 +_LON_MIN = -180 +_LON_RANGE = 360 +_NO_DATA = 0xFF + +# Zlib-compressed 116x360 grid: each byte is a continent index (0-5) or 0xFF. +# Inspect with: python -c "import zlib; print(zlib.decompress(bytes.fromhex('')).hex())" +_MAP: bytes = zlib.decompress( + bytes.fromhex( + '78daed9c8b76e2300c446dc5feff4fd6764948fc90df926d6875763925d0506e86f1580e419c57' + 'f65585271df8e1b5fa0dd8a7927fe24f7dc69bd9f74f2b313e39cf25f2f19f9d12e60afff802e9' + 'ad388e31678b7f25e8cee7bdef77d145a6646bfc79aa371fdf74545bd2c6978d818bf5fc7b7cd9' + '3d94c77cce2b047b2c70cbe358f1aa6bf57c2c9173007a0af55fe91b0b387f3568fd53358a9ecc' + 'd9fe12ce4b3cda7e2f689de4bc42d2839cf76dfae80c675ccbd97e979c31cd79bea26d589ba402' + '0e31a731cf57b41d017d6cca593bf50d9c37368c02e7c589e3d330131c755049ceb89633236b75' + '956ca4f0616a5d037a22658b5618b47a4a9ab36ee4bca2ac1468a5a449474035556b46f23ace96' + '9bb392e79cacf3b97f9c85316f22682b045a2969d05d9c97350aac0c68a5a441eb299c2b0652fd' + 'c7795ae2d069bca2c6a1be93b3decd3894ca91e6c0aed7e85977d3b65680b4ca81e691b73867ed' + '00d5a7514be87988f53e9c3b9dc0ff458de39f0a2b013acd59bd1f9cc3d967796e7918ea3850bc' + '90eaa85d356e3f129c5559cfd71d25cdf942fbfc4aa0e60860b041d7853b46ce0d5dae16ceb2a4' + '5d395fe07da4a1e47da8e7336670b67c72564880be382b11d4be0d6b8fe3d37c7a1ea20c9b2bb5' + 'f073562a0f9a3750570f81977235226d22f17eef657379ca3da055aef8fbff196f26324515e6f7' + '719aa5e52ace47136702b492e28cd4ba969f3de8c8a63d3fd75330f373760c5a09083a27d6386f' + '38c8c9fd629cf36430b31ab4e7d16c33e1383157720e629b50cb691d6785bc9d25ed9c4d901128' + '531296c12c3210febf69db27145bc43ee7d244db5b2527d4ac3f80331638f7cc4f00cbe408cecf' + '9662bb2d98f96926e6d6ae11f493315457d80032814561229a61c4f943c7bbfcf90920da083087' + 'b3e5c7dce11c047278218010c4b91dae7a6f898e1290472e863a0d3327681ecf03b2dc875cccce' + 'a33df80057723e46f41c9847cb9b72b95e386fb608fee3ee6fb0aa548672ab9eab39ab0eed800f' + 'fa75fbe68b10699db095226658c5997d9a82a9ae68416537df87ad2fe4d852ce5d429225508ebc' + '8633cee35c35f0393120f2e548cab7e0d3e28d402f11f4f97426cca155a866cc9e2107631e3d28' + 'ca0e74abc2b3524da0bb4246142a28ca0019caa2a3e0434d6a7e5281b91b74443909faf4148034' + 'cd347f5ece22fdfdbb4134c4194a9c63e4bed673fb816b3b885a4603e7ce571817744e824909a3' + '133baa3e19b26e6deb1d1abb412b3546ba8d333aa2aeb60269cef543e1c07ed518e804035ac04f' + 'cfa75ecc0594bc94ad1867a5c640973823d5b300c8206ad233b798f3826ea3fa4ca05543b5737e' + '106310dbdced2359197809978ca34fbc6d984b9c8108627e1cf6535c2e625c19e3be85a9139671' + 'cb504395e5ec07dca741446adc31109f1860d07e4ef433a64f07711ae546cef1dc02dc0e47ccf9' + 'ddd780802c484eb325300f735699be4ee8108f26c119f39c11d1a3086fd9debd52ff13b098f264' + 'ccaaae2de7408d92723c0b0c8e04f19160c76c9abb1c7331d3a09db934611b7ec870e35ed56876' + 'cd6ba6c93992369eff2661c6cc772748ce107006cf4d2ad1bddb4b535dc3bb1bcfcaa5e57cef24' + 'b36642f8732cdf06cef2dd6652beb92ed37a778e1d97e69cece44f0b15d9011049ce557cd9dc59' + '5573c6b49ed32b262b1afc688cf9ffffae789434c97133d4311be7944153a0a3369d9bd4c6ede1' + '60826c7cc8f4b3eae4cc286892742aab850b2645ce939d83a06c6e8d9f3f67f953b631c0b978c1' + '88b43b2494cebf0255ad6e73cbd398949a4d9dcc99c7c1e2850c48a071f3599273dbd5c04c00f3' + '7d8ff06bc13673b375644e990b5bd0797f805ef83d175d8b31bfa90e30e695b3079a3e0d833ec5' + 'c86b7746243bedb9ef428da164cf1f9dadbda0156b6530e74a62b86b87fc0a6f3e67e39b09f6eb' + '1919e5fc7086d6e2e47cfc1ffe3af29d9f986399e3806dc87086f6e26b0bf5246853cacc66cc9d' + '7b4123af9a230b21873ba863dc39532984b651ca5da031b56588b07b76c1ecb9a009e7d9b49e27' + '734e82877129275287ec8cdb18939f859c87618c3163e60066d0d54de6910be71a9ab371d75836' + 'd0b27b9c00f8152ddfcf48707e3a75e39e71c2668bd11c9cbde5d6359da3a28d74299a71bec2c3' + '196043ce86e7a53ac2b210e7e86cbb35944bed39d325672cafb5aa8a63c224e7ece95f2bb4cc20' + 'e96c1fba55fc0c6ac6a8c9b40f6723651d61022c39cc306748af036c809907343d498b964941aa' + '9c579906da48734eae3221dd209e520e679cc2d9c87356f9d569f21b7c053ee39c630bd98cb319' + '831c7ca9fd0e55751859f48efe978ff34be0cb309b26c72870f6faeef51f774e31cf016d8c31f2' + 'd691027d9f0657cd0ff907c429ad0ed3596c69e3f28d4a8cecc3a4cb19f6e3cc17eabcf90291bd' + '82815334e5096ada740bfa7dde014b7826b44ce613e1382d07da0c161a265da7b2054abac63cd0' + '66b8c438c3a4298bf77d4229d41c9cfb70c356e57c71538474bd19f3ca1a762bafe70f7bc9d9dd' + 'c387630e3b599b9946e7d2a1e090c6226856d086af3edc9ce35e7fdaa35bdf6f95e94a709e9c24' + 'ba66e129d2deb9ca556fdbb0d6c0acfb5338c7a724d7bc71b30767dc9673b4c1e4306e28e7e4b5' + '69f7491bde9a4fa99577075fbe6e28f720486ed8c032fc3f293d221649770a966dda9d6e14c14a' + 'ce905e5ba3834408946be595611a48e879595323abe8ea65f0148d01efe56a22e5dc19970f8381' + '8135078a9ec89cde271d6b2ad107e7b86d9aa549d2ff002e1e877d' + ) +) + + +def lookup_continent(lat: int, lon: int) -> str | None: + """Return the continent directory for a 1-degree tile, or None if unavailable.""" + li = lat - _LAT_MIN + lo = lon - _LON_MIN + if not (0 <= li < _LAT_RANGE and 0 <= lo < _LON_RANGE): + return None + idx = _MAP[li * _LON_RANGE + lo] + if idx == _NO_DATA: + return None + return _CONTINENTS[idx] diff --git a/mavros_extras/mavros_extras/terrain_server_node.py b/mavros_extras/mavros_extras/terrain_server_node.py new file mode 100644 index 000000000..cce25b87a --- /dev/null +++ b/mavros_extras/mavros_extras/terrain_server_node.py @@ -0,0 +1,217 @@ +#!/usr/bin/env python3 +""" +ROS 2 terrain server node. + +Subscribes to terrain requests from the MAVROS terrain plugin, +looks up SRTM elevation data, and publishes terrain data blocks back. +Also provides a service for point elevation queries. +""" + +from __future__ import annotations + +from collections import deque +import threading + +from mavros_extras.srtm import ( + compute_terrain_data_block, + GRID_COLS, + GRID_ROWS, + SrtmManager, +) +from mavros_msgs.msg import TerrainData, TerrainRequest +from mavros_msgs.srv import TerrainCheck +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile + + +class _PendingRequest: + """Tracks which 4x4 blocks have been requested vs already sent.""" + + __slots__ = ('lat', 'lon', 'grid_spacing', 'mask', 'sent_mask') + + def __init__(self, lat: int, lon: int, grid_spacing: int, mask: int) -> None: + self.lat = lat + self.lon = lon + self.grid_spacing = grid_spacing + self.mask = mask + self.sent_mask = 0 + + @property + def remaining(self) -> int: + """Bitmask of blocks still needed.""" + return self.mask & ~self.sent_mask + + +class TerrainServerNode(Node): + """Serves SRTM elevation data in response to MAVLink terrain requests.""" + + def __init__(self) -> None: + super().__init__('terrain_server_node') + + self.declare_parameter('terrain_data_path', '') + self.declare_parameter('auto_download', False) + self.declare_parameter('download_host', 'terrain.ardupilot.org') + self.declare_parameter('srtm_source', 'SRTM3') + self.declare_parameter('send_rate_hz', 5.0) + self.declare_parameter('max_cache_tiles', 64) + + terrain_data_path = self.get_parameter('terrain_data_path').value + auto_download = self.get_parameter('auto_download').value + download_host = self.get_parameter('download_host').value + srtm_source = self.get_parameter('srtm_source').value + rate_hz = self.get_parameter('send_rate_hz').value + max_cache_tiles = self.get_parameter('max_cache_tiles').value + + if rate_hz <= 0.0: + rate_hz = 5.0 + + self._mgr = SrtmManager( + terrain_data_path=terrain_data_path, + auto_download=auto_download, + download_host=download_host, + srtm_source=srtm_source, + max_cache_tiles=max_cache_tiles, + ) + + self._pending: deque[_PendingRequest] = deque() + self._lock = threading.Lock() + + self._blocks_served = 0 + self._requests_received = 0 + + self._data_pub = self.create_publisher( + TerrainData, + '/mavros/terrain/data', + QoSProfile(depth=64), + ) + + self.create_subscription( + TerrainRequest, + '/mavros/terrain/request', + self._on_request, + QoSProfile(depth=10), + ) + + self.create_service( + TerrainCheck, + '/mavros/terrain/check', + self._on_check, + ) + + period = 1.0 / rate_hz + self._timer = self.create_timer(period, self._on_send_tick) + + self.get_logger().info( + f'Terrain server ready path={terrain_data_path or "(none)"}' + f' auto_download={auto_download} rate={rate_hz:.1f} Hz' + ) + + # ---------------------------------------------------------------- callbacks + + def _on_request(self, msg: TerrainRequest) -> None: + lat_deg = msg.lat / 1e7 + lon_deg = msg.lon / 1e7 + + with self._lock: + for req in self._pending: + if ( + req.lat == msg.lat + and req.lon == msg.lon + and req.grid_spacing == msg.grid_spacing + ): + req.mask = msg.mask + req.sent_mask &= msg.mask + self.get_logger().debug( + f'Updated pending request lat={lat_deg:.7f}' + f' lon={lon_deg:.7f} mask=0x{msg.mask:016x}' + ) + return + + self._pending.append(_PendingRequest(msg.lat, msg.lon, msg.grid_spacing, msg.mask)) + self._requests_received += 1 + count = self._requests_received + + self.get_logger().info( + f'TERRAIN_REQUEST #{count} lat={lat_deg:.7f} lon={lon_deg:.7f}' + f' spacing={msg.grid_spacing} mask=0x{msg.mask:016x}' + ) + + def _on_check( + self, + request: TerrainCheck.Request, + response: TerrainCheck.Response, + ) -> TerrainCheck.Response: + elev = self._mgr.lookup_elevation(request.latitude, request.longitude) + if elev is None: + response.success = False + response.terrain_height = 0.0 + self.get_logger().debug( + f'Check: no data at ({request.latitude:.7f}, {request.longitude:.7f})' + ) + else: + response.success = True + response.terrain_height = float(elev) + return response + + # ---------------------------------------------------------------- timer + + def _on_send_tick(self) -> None: + with self._lock: + while self._pending and self._pending[0].remaining == 0: + self._pending.popleft() + + if not self._pending: + return + + req = self._pending[0] + needed = req.remaining + + for bit in range(GRID_COLS * GRID_ROWS): + if not (needed & (1 << bit)): + continue + + data = compute_terrain_data_block(self._mgr, req.lat, req.lon, req.grid_spacing, bit) + if data is None: + self.get_logger().debug( + f'No elevation data for bit {bit}' + f' at ({req.lat / 1e7:.7f}, {req.lon / 1e7:.7f})' + ) + continue + + msg = TerrainData() + msg.lat = req.lat + msg.lon = req.lon + msg.grid_spacing = req.grid_spacing + msg.gridbit = bit + msg.data = data + + self._data_pub.publish(msg) + + with self._lock: + req.sent_mask |= 1 << bit + self._blocks_served += 1 + if req.remaining == 0: + self.get_logger().info( + f'Completed terrain request lat={req.lat / 1e7:.7f}' + f' lon={req.lon / 1e7:.7f}' + f' ({self._blocks_served} blocks served total)' + ) + + return + + +def main(args=None) -> None: + rclpy.init(args=args) + node = TerrainServerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/mavros_extras/mavros_plugins.xml b/mavros_extras/mavros_plugins.xml index 9fc2a437c..1f8eae581 100644 --- a/mavros_extras/mavros_plugins.xml +++ b/mavros_extras/mavros_plugins.xml @@ -220,10 +220,21 @@ Published topics (relative to plugin namespace): @plugin tdr_radio - @brief Terrain height plugin. + @brief Terrain plugin. @plugin terrain -This plugin allows publishing of terrain height estimate from FCU to ROS. +Handles the full MAVLink terrain protocol (TERRAIN_REPORT, +TERRAIN_REQUEST, TERRAIN_CHECK, TERRAIN_DATA). + +Published topics (relative to plugin namespace): +- ~/report (mavros_msgs/TerrainReport): terrain height from FCU and check responses +- ~/request (mavros_msgs/TerrainRequest): FCU grid data requests + +Subscribed topics: +- ~/data (mavros_msgs/TerrainData): filled grid blocks from terrain server node + +Service clients: +- ~/check (mavros_msgs/TerrainCheck): point elevation query @brief Trajectory plugin to receive planned path from the FCU and @@ -268,4 +279,4 @@ This plugin allows computing and publishing wheel odometry coming from FCU wheel Can use either wheel's RPM or WHEEL_DISTANCE messages (the latter gives better accuracy). - + diff --git a/mavros_extras/package.xml b/mavros_extras/package.xml index b70822bc2..b7635fa6e 100644 --- a/mavros_extras/package.xml +++ b/mavros_extras/package.xml @@ -34,7 +34,6 @@ geographiclib geographiclib-tools geographiclib-tools - angles diagnostic_updater @@ -64,14 +63,17 @@ std_srvs visualization_msgs + rclpy rosidl_default_runtime ament_cmake_gtest ament_cmake_gmock + ament_cmake_pytest ament_lint_auto ament_lint_common gtest google-mock + python3-pytest ament_cmake diff --git a/mavros_extras/ruff.toml b/mavros_extras/ruff.toml new file mode 100644 index 000000000..9483d9397 --- /dev/null +++ b/mavros_extras/ruff.toml @@ -0,0 +1,7 @@ +line-length = 99 + +[format] +quote-style = "single" + +[lint.pycodestyle] +max-line-length = 99 diff --git a/mavros_extras/scripts/terrain_tile_server b/mavros_extras/scripts/terrain_tile_server new file mode 100755 index 000000000..cee6486aa --- /dev/null +++ b/mavros_extras/scripts/terrain_tile_server @@ -0,0 +1,6 @@ +#!/usr/bin/env python3 +"""Entry-point for ``ros2 run mavros_extras terrain_tile_server``.""" + +from mavros_extras.terrain_server_node import main + +main() diff --git a/mavros_extras/setup.cfg b/mavros_extras/setup.cfg new file mode 100644 index 000000000..9403999cc --- /dev/null +++ b/mavros_extras/setup.cfg @@ -0,0 +1,35 @@ +[metadata] +name = 'mavros_extras' +description = 'Extra nodes and plugins for MAVROS' +license = 'Triple licensed under GPLv3, LGPLv3 and BSD' +author = 'Vladimir Ermakov' +author_email = 'vooon341@gmail.com' +maintainer = 'Vladimir Ermakov' +maintainer_email = 'vooon341@gmail.com' +keywords = 'ROS' +classifiers = + 'Intended Audience :: Developers' + 'License :: OSI Approved :: GNU General Public License v3 (GPLv3)' + 'License :: OSI Approved :: GNU Lesser General Public License v3 (LGPLv3)' + 'License :: OSI Approved :: BSD License' + 'Programming Language :: Python' + 'Topic :: Software Development' +tests_require = 'pytest' + +[options.entry_points] +console_scripts = + terrain_tile_server=mavros_extras.terrain_server_node:main + +[develop] +script_dir=$base/lib/mavros_extras + +[install] +install_scripts=$base/lib/mavros_extras + +[flake8] +# NOTE: based on ament_flake8.ini from Jazzy, but extended with Q000,I100,I101 +extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,Q000,I100,I101 +import-order-style = google +max-line-length = 99 +show-source = true +statistics = true diff --git a/mavros_extras/setup.py b/mavros_extras/setup.py new file mode 100644 index 000000000..912c7333c --- /dev/null +++ b/mavros_extras/setup.py @@ -0,0 +1,28 @@ +from setuptools import setup + +package_name = 'mavros_extras' + +setup( + name=package_name, + version='2.14.0', + packages=[package_name], + install_requires=['setuptools'], + zip_safe=True, + author='Vladimir Ermakov', + author_email='vooon341@gmail.com', + maintainer='Vladimir Ermakov', + maintainer_email='vooon341@gmail.com', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'Programming Language :: Python', + ], + description='Extra nodes and plugins for MAVROS', + license='Triple licensed under GPLv3, LGPLv3 and BSD', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'terrain_tile_server = mavros_extras.terrain_server_node:main', + ], + }, +) diff --git a/mavros_extras/src/plugins/terrain.cpp b/mavros_extras/src/plugins/terrain.cpp index 428fcbfd2..0b565d62f 100644 --- a/mavros_extras/src/plugins/terrain.cpp +++ b/mavros_extras/src/plugins/terrain.cpp @@ -1,5 +1,6 @@ /* * Copyright 2021 Ardupilot. + * Copyright 2026 Zeke Sarosi. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. @@ -9,9 +10,32 @@ * @brief Terrain plugin * @file terrain.cpp * @author Matt Anderson + * @author Zeke Sarosi * * @addtogroup plugin * @{ + * + * Handles the complete MAVLink terrain protocol: + * + * - TERRAIN_REPORT (FCU → ROS): publishes terrain height estimates + * - TERRAIN_REQUEST (FCU → ROS): forwards grid requests to the terrain + * server node for SRTM lookup + * - TERRAIN_CHECK (FCU → ROS → FCU): point elevation query via service, + * responds with TERRAIN_REPORT + * - TERRAIN_DATA (ROS → FCU): forwards filled grid blocks back to the FCU + * + * All heavy lifting (SRTM tile download, caching, elevation lookup) + * is handled by a separate terrain_server_node (Python). + * + * Published topics: + * - ~/report (mavros_msgs/TerrainReport) + * - ~/request (mavros_msgs/TerrainRequest) + * + * Subscribed topics: + * - ~/data (mavros_msgs/TerrainData) + * + * Service clients: + * - ~/check (mavros_msgs/TerrainCheck) */ #include "rcpputils/asserts.hpp" @@ -19,7 +43,10 @@ #include "mavros/plugin.hpp" #include "mavros/plugin_filter.hpp" +#include "mavros_msgs/msg/terrain_data.hpp" #include "mavros_msgs/msg/terrain_report.hpp" +#include "mavros_msgs/msg/terrain_request.hpp" +#include "mavros_msgs/srv/terrain_check.hpp" namespace mavros { @@ -28,11 +55,21 @@ namespace extra_plugins using namespace std::placeholders; // NOLINT /** - * @brief Terrain height plugin. + * @brief Terrain plugin. * @plugin terrain * - * This plugin allows publishing of terrain height estimate from FCU to ROS. + * Handles the full MAVLink terrain protocol (TERRAIN_REPORT, + * TERRAIN_REQUEST, TERRAIN_CHECK, TERRAIN_DATA). + * + * Published topics (relative to plugin namespace): + * - ~/report (mavros_msgs/TerrainReport): terrain height from FCU and check responses + * - ~/request (mavros_msgs/TerrainRequest): FCU grid data requests * + * Subscribed topics: + * - ~/data (mavros_msgs/TerrainData): filled grid blocks from terrain server node + * + * Service clients: + * - ~/check (mavros_msgs/TerrainCheck): point elevation query */ class TerrainPlugin : public plugin::Plugin { @@ -40,38 +77,134 @@ class TerrainPlugin : public plugin::Plugin explicit TerrainPlugin(plugin::UASPtr uas_) : Plugin(uas_, "terrain") { - terrain_report_pub = node->create_publisher("~/report", 10); + // Terrain height reports from FCU and check-service responses + report_pub_ = node->create_publisher( + "~/report", 10); + // Grid data requests forwarded from FCU for SRTM lookup + request_pub_ = node->create_publisher( + "~/request", 10); + + // Filled terrain grid blocks from terrain_tile_server + data_sub_ = node->create_subscription( + "~/data", 64, + std::bind(&TerrainPlugin::data_cb, this, _1)); + + // Point elevation query handled by terrain_tile_server + check_client_ = node->create_client( + "~/check"); } Subscriptions get_subscriptions() override { return { - make_handler(&TerrainPlugin::handle_terrain_report) + make_handler(&TerrainPlugin::handle_terrain_report), + make_handler(&TerrainPlugin::handle_terrain_request), + make_handler(&TerrainPlugin::handle_terrain_check), }; } private: - rclcpp::Publisher::SharedPtr terrain_report_pub; + rclcpp::Publisher::SharedPtr report_pub_; + rclcpp::Publisher::SharedPtr request_pub_; + rclcpp::Subscription::SharedPtr data_sub_; + rclcpp::Client::SharedPtr check_client_; void handle_terrain_report( const mavlink::mavlink_message_t * msg [[maybe_unused]], mavlink::common::msg::TERRAIN_REPORT & report, plugin::filter::SystemAndOk filter [[maybe_unused]]) { - auto terrain_report_msg = mavros_msgs::msg::TerrainReport(); + auto ros_msg = mavros_msgs::msg::TerrainReport(); + ros_msg.header.stamp = node->now(); + ros_msg.header.frame_id = "terrain"; + ros_msg.latitude = static_cast(report.lat) / 1e7; + ros_msg.longitude = static_cast(report.lon) / 1e7; + ros_msg.spacing = report.spacing; + ros_msg.terrain_height = report.terrain_height; + ros_msg.current_height = report.current_height; + ros_msg.pending = report.pending; + ros_msg.loaded = report.loaded; - terrain_report_msg.header.stamp = node->now(); - terrain_report_msg.header.frame_id = "terrain"; + report_pub_->publish(ros_msg); + } - terrain_report_msg.latitude = static_cast(report.lat) / 1e7; - terrain_report_msg.longitude = static_cast(report.lon) / 1e7; - terrain_report_msg.spacing = report.spacing; - terrain_report_msg.terrain_height = report.terrain_height; - terrain_report_msg.current_height = report.current_height; - terrain_report_msg.pending = report.pending; - terrain_report_msg.loaded = report.loaded; + void handle_terrain_request( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::TERRAIN_REQUEST & request, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto ros_msg = mavros_msgs::msg::TerrainRequest(); + ros_msg.header.stamp = node->now(); + ros_msg.header.frame_id = "terrain"; + ros_msg.lat = request.lat; + ros_msg.lon = request.lon; + ros_msg.grid_spacing = request.grid_spacing; + ros_msg.mask = request.mask; + + request_pub_->publish(ros_msg); + } + + void handle_terrain_check( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::TERRAIN_CHECK & check, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + if (!check_client_->service_is_ready()) { + RCLCPP_WARN_THROTTLE(get_logger(), *node->get_clock(), 5000, + "terrain/check service not available"); + return; + } + + auto req = std::make_shared(); + req->latitude = check.lat / 1e7; + req->longitude = check.lon / 1e7; + + check_client_->async_send_request(req, + [this, check]( + rclcpp::Client::SharedFuture future) + { + auto result = future.get(); + if (!result || !result->success) { + RCLCPP_WARN(get_logger(), + "TERRAIN_CHECK: no data for lat=%.7f lon=%.7f", + check.lat / 1e7, check.lon / 1e7); + return; + } + + mavlink::common::msg::TERRAIN_REPORT rpt{}; + rpt.lat = check.lat; + rpt.lon = check.lon; + rpt.spacing = 0; + rpt.terrain_height = result->terrain_height; + rpt.current_height = 0; + rpt.pending = 0; + rpt.loaded = 1; + uas->send_message(rpt); + + auto ros_rpt = mavros_msgs::msg::TerrainReport(); + ros_rpt.header.stamp = node->now(); + ros_rpt.header.frame_id = "terrain"; + ros_rpt.latitude = check.lat / 1e7; + ros_rpt.longitude = check.lon / 1e7; + ros_rpt.spacing = 0; + ros_rpt.terrain_height = result->terrain_height; + ros_rpt.current_height = 0; + ros_rpt.pending = 0; + ros_rpt.loaded = 1; + report_pub_->publish(ros_rpt); + }); + } + + void data_cb(const mavros_msgs::msg::TerrainData::SharedPtr msg) + { + mavlink::common::msg::TERRAIN_DATA td{}; + td.lat = msg->lat; + td.lon = msg->lon; + td.grid_spacing = msg->grid_spacing; + td.gridbit = msg->gridbit; + std::copy(msg->data.begin(), msg->data.end(), std::begin(td.data)); - terrain_report_pub->publish(terrain_report_msg); + uas->send_message(td); } }; } // namespace extra_plugins diff --git a/mavros_extras/test/test_srtm.py b/mavros_extras/test/test_srtm.py new file mode 100644 index 000000000..6d24c265c --- /dev/null +++ b/mavros_extras/test/test_srtm.py @@ -0,0 +1,307 @@ +"""Unit tests for mavros_extras.srtm module.""" + +from __future__ import annotations + +import array +import struct + +from mavros_extras.srtm import ( + compute_terrain_data_block, + gps_newpos, + gps_offset, + GRID_COLS, + GRID_ROWS, + SRTM1_SIDE, + SRTM3_SIDE, + SRTM_VOID, + SrtmManager, + SrtmTile, + TILE_DIM, +) +import pytest + + +# ------------------------------------------------------------------ SrtmTile + + +class TestSrtmTile: + """Test SrtmTile data class.""" + + def test_repr(self): + tile = SrtmTile(array.array('h', [0]), 1201) + assert 'SrtmTile(side=1201)' == repr(tile) + + def test_slots(self): + tile = SrtmTile(array.array('h', [42]), 3601) + assert tile.side == 3601 + assert tile.data[0] == 42 + + +# ------------------------------------------------------------------ tile key / filename + + +class TestTileKeyFilename: + """Test tile key and filename generation.""" + + def test_key_unique(self): + keys = {SrtmManager._tile_key(lat, lon) for lat in (-90, 0, 89) for lon in (-180, 0, 179)} + assert len(keys) == 9 + + def test_key_deterministic(self): + assert SrtmManager._tile_key(47, 11) == SrtmManager._tile_key(47, 11) + + @pytest.mark.parametrize( + ('lat', 'lon', 'expected'), + [ + (47, 11, 'N47E011.hgt'), + (-34, -58, 'S34W058.hgt'), + (0, 0, 'N00E000.hgt'), + (-1, -1, 'S01W001.hgt'), + (89, 179, 'N89E179.hgt'), + (-90, -180, 'S90W180.hgt'), + ], + ) + def test_tile_filename(self, lat, lon, expected): + assert SrtmManager._tile_filename(lat, lon) == expected + + +# ------------------------------------------------------------------ synthetic tile helpers + + +def _make_flat_tile(side: int, elevation: int = 500) -> bytes: + """Create a synthetic .hgt file with uniform elevation.""" + n = side * side + return struct.pack(f'>{n}h', *([elevation] * n)) + + +def _make_gradient_tile(side: int) -> bytes: + """Create a tile where elevation = row index (south-to-north gradient).""" + data = [] + for row in range(side): + data.extend([row] * side) + return struct.pack(f'>{side * side}h', *data) + + +def _make_void_tile(side: int) -> bytes: + """Create a tile filled with SRTM void markers.""" + n = side * side + return struct.pack(f'>{n}h', *([SRTM_VOID] * n)) + + +# ------------------------------------------------------------------ SrtmManager load + + +class TestSrtmManagerLoad: + """Test tile loading from disk.""" + + def test_load_srtm3_flat(self, tmp_path): + hgt = tmp_path / 'N47E011.hgt' + hgt.write_bytes(_make_flat_tile(SRTM3_SIDE, 800)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + elev = mgr.lookup_elevation(47.5, 11.5) + assert elev is not None + assert abs(elev - 800.0) < 1e-6 + + def test_load_srtm1_flat(self, tmp_path): + hgt = tmp_path / 'N47E011.hgt' + hgt.write_bytes(_make_flat_tile(SRTM1_SIDE, 1200)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + elev = mgr.lookup_elevation(47.5, 11.5) + assert elev is not None + assert abs(elev - 1200.0) < 1e-6 + + def test_load_missing_tile(self, tmp_path): + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + assert mgr.lookup_elevation(47.5, 11.5) is None + + def test_load_bad_file_size(self, tmp_path): + hgt = tmp_path / 'N47E011.hgt' + hgt.write_bytes(b'\x00' * 100) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + assert mgr.lookup_elevation(47.5, 11.5) is None + + def test_no_data_path(self): + mgr = SrtmManager(terrain_data_path='') + assert mgr.lookup_elevation(47.5, 11.5) is None + + +# ------------------------------------------------------------------ elevation lookup + + +class TestElevationLookup: + """Test bilinear elevation interpolation.""" + + def test_corner_exact(self, tmp_path): + """Elevation at exact SW corner of the tile.""" + hgt = tmp_path / 'N00E000.hgt' + hgt.write_bytes(_make_flat_tile(SRTM3_SIDE, 100)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + elev = mgr.lookup_elevation(0.0, 0.0) + assert elev is not None + assert abs(elev - 100.0) < 1e-6 + + def test_bilinear_interpolation(self, tmp_path): + """With a gradient tile, mid-row should interpolate.""" + hgt = tmp_path / 'N00E000.hgt' + hgt.write_bytes(_make_gradient_tile(SRTM3_SIDE)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + elev = mgr.lookup_elevation(0.5, 0.5) + assert elev is not None + expected_row = (1.0 - 0.5) * (SRTM3_SIDE - 1) + assert abs(elev - expected_row) < 1.0 + + def test_void_returns_none(self, tmp_path): + """All-void tile should return None.""" + hgt = tmp_path / 'N10E020.hgt' + hgt.write_bytes(_make_void_tile(SRTM3_SIDE)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + assert mgr.lookup_elevation(10.5, 20.5) is None + + +# ------------------------------------------------------------------ LRU cache + + +class TestLRUCache: + """Test LRU tile cache behavior.""" + + def test_cache_eviction(self, tmp_path): + for lat in range(5): + hgt = tmp_path / f'N{lat:02d}E000.hgt' + hgt.write_bytes(_make_flat_tile(SRTM3_SIDE, lat * 100)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path), max_cache_tiles=2) + + assert mgr.lookup_elevation(0.5, 0.5) is not None + assert mgr.lookup_elevation(1.5, 0.5) is not None + assert mgr.lookup_elevation(2.5, 0.5) is not None + + assert len(mgr._cache) <= 2 + + def test_cache_hit_returns_same(self, tmp_path): + hgt = tmp_path / 'N05E005.hgt' + hgt.write_bytes(_make_flat_tile(SRTM3_SIDE, 333)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + e1 = mgr.lookup_elevation(5.5, 5.5) + e2 = mgr.lookup_elevation(5.5, 5.5) + assert e1 == e2 + + +# ------------------------------------------------------------------ gps_newpos / gps_offset + + +class TestGeodesic: + """Test rhumb-line geodesic helpers.""" + + def test_zero_distance(self): + lat, lon = gps_newpos(47.0, 11.0, 90.0, 0.0) + assert lat == 47.0 + assert lon == 11.0 + + def test_north(self): + lat, lon = gps_newpos(0.0, 0.0, 0.0, 111_320.0) + assert lat == pytest.approx(1.0, abs=0.02) + assert lon == pytest.approx(0.0, abs=1e-6) + + def test_east(self): + lat, lon = gps_newpos(0.0, 0.0, 90.0, 111_320.0) + assert lat == pytest.approx(0.0, abs=0.02) + assert lon == pytest.approx(1.0, abs=0.02) + + def test_south(self): + lat, lon = gps_newpos(10.0, 0.0, 180.0, 111_320.0) + assert lat == pytest.approx(9.0, abs=0.02) + + def test_symmetry(self): + lat1, lon1 = gps_newpos(45.0, 10.0, 45.0, 10000.0) + lat2, lon2 = gps_newpos(lat1, lon1, 225.0, 10000.0) + assert lat2 == pytest.approx(45.0, abs=0.001) + assert lon2 == pytest.approx(10.0, abs=0.001) + + def test_gps_offset_north(self): + lat, lon = gps_offset(0.0, 0.0, 0.0, 111_320.0) + assert lat == pytest.approx(1.0, abs=0.02) + + def test_gps_offset_east(self): + lat, lon = gps_offset(0.0, 0.0, 111_320.0, 0.0) + assert lon == pytest.approx(1.0, abs=0.02) + + +# ------------------------------------------------------------------ compute_terrain_data_block + + +class TestComputeTerrainDataBlock: + """Test MAVLink terrain data block computation.""" + + def test_flat_tile_block(self, tmp_path): + hgt = tmp_path / 'N47E011.hgt' + hgt.write_bytes(_make_flat_tile(SRTM3_SIDE, 600)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + block = compute_terrain_data_block( + mgr, lat_e7=470_000_000, lon_e7=110_000_000, grid_spacing=100, bit=0 + ) + assert block is not None + assert len(block) == TILE_DIM * TILE_DIM + assert all(v == 600 for v in block) + + def test_missing_tile_returns_none(self, tmp_path): + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + block = compute_terrain_data_block( + mgr, lat_e7=470_000_000, lon_e7=110_000_000, grid_spacing=100, bit=0 + ) + assert block is None + + def test_block_size(self, tmp_path): + hgt = tmp_path / 'N47E011.hgt' + hgt.write_bytes(_make_flat_tile(SRTM3_SIDE, 400)) + + mgr = SrtmManager(terrain_data_path=str(tmp_path)) + for bit in range(min(4, GRID_COLS * GRID_ROWS)): + block = compute_terrain_data_block( + mgr, lat_e7=470_000_000, lon_e7=110_000_000, grid_spacing=30, bit=bit + ) + if block is not None: + assert len(block) == 16 + + def test_grid_constants(self): + assert GRID_COLS == 8 + assert GRID_ROWS == 7 + assert TILE_DIM == 4 + + +# ------------------------------------------------------------------ continent map + + +class TestContinentMap: + """Test continent lookup table.""" + + def test_import(self): + from mavros_extras.srtm_continent_map import lookup_continent + + assert callable(lookup_continent) + + def test_known_tiles(self): + from mavros_extras.srtm_continent_map import lookup_continent + + result = lookup_continent(47, 11) + assert result is not None + assert 'Eurasia' in result + + def test_out_of_range(self): + from mavros_extras.srtm_continent_map import lookup_continent + + assert lookup_continent(90, 0) is None + assert lookup_continent(0, 360) is None + + def test_ocean_tile(self): + from mavros_extras.srtm_continent_map import lookup_continent + + result = lookup_continent(0, -30) + assert result is None diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index 1f8b44b3c..15d11c5db 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -99,7 +99,9 @@ set(msg_files msg/StatusEvent.msg msg/StatusText.msg msg/SysStatus.msg + msg/TerrainData.msg msg/TerrainReport.msg + msg/TerrainRequest.msg msg/Thrust.msg msg/TimesyncStatus.msg msg/Trajectory.msg @@ -111,7 +113,7 @@ set(msg_files msg/WaypointList.msg msg/WaypointReached.msg msg/WheelOdomStamped.msg - # [[[end]]] (sum: 4tpSf/CPJJ) + # [[[end]]] (sum: WNC7AFvjZ5) ) set(srv_files @@ -159,12 +161,13 @@ set(srv_files srv/SetMavFrame.srv srv/SetMode.srv srv/StreamRate.srv + srv/TerrainCheck.srv srv/VehicleInfoGet.srv srv/WaypointClear.srv srv/WaypointPull.srv srv/WaypointPush.srv srv/WaypointSetCurrent.srv - # [[[end]]] (sum: zXcBsooxdt) + # [[[end]]] (sum: gwjU4eY/S4) ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/mavros_msgs/msg/TerrainData.msg b/mavros_msgs/msg/TerrainData.msg new file mode 100644 index 000000000..307ed2d74 --- /dev/null +++ b/mavros_msgs/msg/TerrainData.msg @@ -0,0 +1,8 @@ +# MAVLink TERRAIN_DATA +# https://mavlink.io/en/messages/common.html#TERRAIN_DATA + +int32 lat # Latitude degE7 +int32 lon # Longitude degE7 +uint16 grid_spacing # Grid spacing in meters +uint8 gridbit # Index of the 4x4 block inside the grid +int16[16] data # Terrain elevation values in meters diff --git a/mavros_msgs/msg/TerrainRequest.msg b/mavros_msgs/msg/TerrainRequest.msg new file mode 100644 index 000000000..096f4302d --- /dev/null +++ b/mavros_msgs/msg/TerrainRequest.msg @@ -0,0 +1,13 @@ +# MAVLink TERRAIN_REQUEST +# https://mavlink.io/en/messages/common.html#TERRAIN_REQUEST +# +# NOTE: lat/lon use int32 degE7 to match the MAVLink wire format exactly. +# The terrain server must echo these values back in TERRAIN_DATA without +# modification; converting through float64 risks rounding mismatches. + +std_msgs/Header header + +int32 lat # Latitude degE7 +int32 lon # Longitude degE7 +uint16 grid_spacing # Grid spacing in meters +uint64 mask # Bitmask of requested 4x4 terrain blocks diff --git a/mavros_msgs/srv/TerrainCheck.srv b/mavros_msgs/srv/TerrainCheck.srv new file mode 100644 index 000000000..20c043899 --- /dev/null +++ b/mavros_msgs/srv/TerrainCheck.srv @@ -0,0 +1,8 @@ +# Point elevation query (mirrors MAVLink TERRAIN_CHECK / TERRAIN_REPORT) +# https://mavlink.io/en/messages/common.html#TERRAIN_CHECK + +float64 latitude # WGS-84 degrees +float64 longitude # WGS-84 degrees +--- +bool success +float32 terrain_height # meters AMSL diff --git a/tools/generate_srtm_continent_map.py b/tools/generate_srtm_continent_map.py new file mode 100755 index 000000000..adfecee71 --- /dev/null +++ b/tools/generate_srtm_continent_map.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 +"""Generate srtm_continent_map.py from terrain.ardupilot.org directory listings. + +Crawls the SRTM3 directory on the ArduPilot terrain server, discovers which +continent subdirectory each .hgt.zip tile lives in, and emits a Python module +with a zlib-compressed hex lookup table. + +Usage (from repo root): + python tools/generate_srtm_continent_map.py # default output + python tools/generate_srtm_continent_map.py output.py # custom output + +Requires only the standard library (urllib, re, zlib). + +This script only needs to be run once. The SRTM dataset was collected +in 2000 and the server layout has not changed since. +""" + +from __future__ import annotations + +import re +import sys +import urllib.request +import zlib +from pathlib import Path + +SERVER = "terrain.ardupilot.org" +SOURCE = "SRTM3" +BASE_URL = f"https://{SERVER}/{SOURCE}" + +CONTINENTS = [ + "Africa", + "Australia", + "Eurasia", + "Islands", + "North_America", + "South_America", +] + +LAT_MIN, LAT_MAX = -56, 59 +LON_MIN, LON_MAX = -180, 179 +LAT_RANGE = LAT_MAX - LAT_MIN + 1 # 116 +LON_RANGE = LON_MAX - LON_MIN + 1 # 360 +NO_DATA = 0xFF + +_TILE_RE = re.compile(r'href="([NS]\d{2}[EW]\d{3})\.hgt\.zip"') + + +def _fetch_tile_map() -> bytearray: + """Crawl the server and build a flat lat*lon -> continent-index map.""" + grid = bytearray([NO_DATA] * (LAT_RANGE * LON_RANGE)) + total = 0 + + for ci, continent in enumerate(CONTINENTS): + url = f"{BASE_URL}/{continent}/" + print(f" Scanning {continent:15s} ... ", end="", flush=True) + + try: + with urllib.request.urlopen( # nosemgrep: dynamic-urllib-use-detected + url, timeout=120 + ) as resp: + html = resp.read().decode("utf-8", errors="replace") + except Exception as e: + print(f"FAILED ({e})") + continue + + count = 0 + for match in _TILE_RE.finditer(html): + name = match.group(1) + lat = int(name[1:3]) * (-1 if name[0] == "S" else 1) + lon = int(name[4:7]) * (-1 if name[3] == "W" else 1) + + li = lat - LAT_MIN + lo = lon - LON_MIN + if 0 <= li < LAT_RANGE and 0 <= lo < LON_RANGE: + grid[li * LON_RANGE + lo] = ci + count += 1 + total += 1 + + print(f"{count} tiles") + + if total < 10000: + print( + f"ERROR: only {total} tiles found (expected ~14000) -- server may be down", + file=sys.stderr, + ) + sys.exit(1) + + print(f" Total tiles: {total}") + return grid + + +def _format_hex_literal(grid: bytearray) -> str: + """Compress the grid and return indented hex string lines.""" + compressed = zlib.compress(bytes(grid), level=9) + hex_str = compressed.hex() + lines = [hex_str[i : i + 78] for i in range(0, len(hex_str), 78)] + return "\n".join(f" '{line}'" for line in lines) + + +_MODULE_TEMPLATE = '''\ +""" +Auto-generated SRTM continent lookup table. + +Maps each 1-degree tile to its continent directory on terrain.ardupilot.org. +Generated by ../../tools/generate_srtm_continent_map.py -- DO NOT EDIT. +""" + +from __future__ import annotations + +import zlib + +_CONTINENTS = ( + 'Africa/', + 'Australia/', + 'Eurasia/', + 'Islands/', + 'North_America/', + 'South_America/', +) + +_LAT_MIN = {lat_min} +_LAT_RANGE = {lat_range} +_LON_MIN = {lon_min} +_LON_RANGE = {lon_range} +_NO_DATA = 0xFF + +# Zlib-compressed 116x360 grid: each byte is a continent index (0-5) or 0xFF. +# Inspect with: python -c "import zlib; print(zlib.decompress(bytes.fromhex('')).hex())" +_MAP: bytes = zlib.decompress( + bytes.fromhex( +{hex_literal} + ) +) + + +def lookup_continent(lat: int, lon: int) -> str | None: + """Return the continent directory for a 1-degree tile, or None if unavailable.""" + li = lat - _LAT_MIN + lo = lon - _LON_MIN + if not (0 <= li < _LAT_RANGE and 0 <= lo < _LON_RANGE): + return None + idx = _MAP[li * _LON_RANGE + lo] + if idx == _NO_DATA: + return None + return _CONTINENTS[idx] +''' + + +def _write_module(grid: bytearray, output: Path) -> None: + hex_literal = _format_hex_literal(grid) + content = _MODULE_TEMPLATE.format( + lat_min=LAT_MIN, + lat_range=LAT_RANGE, + lon_min=LON_MIN, + lon_range=LON_RANGE, + hex_literal=hex_literal, + ) + output.write_text(content) + print(f" Wrote {output}") + + +def main() -> None: + script_dir = Path(__file__).resolve().parent + repo_dir = script_dir.parent + default_output = repo_dir / "mavros_extras" / "mavros_extras" / "srtm_continent_map.py" + output = Path(sys.argv[1]) if len(sys.argv) > 1 else default_output + + if output.exists(): + print(f"SRTM continent map already exists at {output}") + print(" Delete the file to regenerate.") + return + + print("Generating SRTM continent map") + print(f" server: {BASE_URL}") + print(f" output: {output}") + + grid = _fetch_tile_map() + _write_module(grid, output) + + +if __name__ == "__main__": + main()