diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim index b6fd1dedcfa..d802fc376b9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim @@ -20,7 +20,13 @@ if [ -n "${PX4_SIM_GPS_USED}" ]; then param set SIM_GPS_USED ${PX4_SIM_GPS_USED} fi -if simulator_sih start; then +sih_args="" +if [ -n "${PX4_SIH_TERRAIN}" ]; then + # terrain elevation GeoTIFF (DEM) + sih_args="-t ${PX4_SIH_TERRAIN}" +fi + +if simulator_sih start ${sih_args}; then if param compare -s SENS_EN_BAROSIM 1 then diff --git a/Tools/setup/arch.sh b/Tools/setup/arch.sh index 972761e31d7..d72be01f45d 100755 --- a/Tools/setup/arch.sh +++ b/Tools/setup/arch.sh @@ -96,6 +96,13 @@ if [[ $INSTALL_SIM == "true" ]]; then echo echo "Installing PX4 simulation dependencies" + # SIH terrain elevation (GeoTIFF DEM) support + sudo pacman -S --noconfirm --needed \ + libgeotiff \ + libtiff \ + proj \ + ; + # Gazebo setup if [[ $INSTALL_GAZEBO == "true" ]]; then echo diff --git a/Tools/setup/macos.sh b/Tools/setup/macos.sh index 2cf37f79c39..092415a3f89 100755 --- a/Tools/setup/macos.sh +++ b/Tools/setup/macos.sh @@ -140,6 +140,7 @@ if [[ $INSTALL_SIM == "--sim-tools" ]]; then exiftool glog graphviz + libgeotiff # SIH terrain elevation (GeoTIFF DEM), pulls libtiff and proj pixi ) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index c947aff2b25..a657f650e21 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -216,6 +216,9 @@ if [[ $INSTALL_SIM == "true" ]]; then # General simulation dependencies sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ bc \ + libgeotiff-dev \ + libproj-dev \ + libtiff-dev \ ; # Gazebo Harmonic installation (Ubuntu 22.04+) diff --git a/docs/en/sim_sih/index.md b/docs/en/sim_sih/index.md index 48a3aeca50c..5bd45febfda 100644 --- a/docs/en/sim_sih/index.md +++ b/docs/en/sim_sih/index.md @@ -163,6 +163,49 @@ Run it from the PX4 shell, or the [MAVLink shell](../debug/mavlink_shell.md), af `orient` is only built for SITL: sensor calibration is rejected when SIH runs on flight controller hardware. +#### Terrain Elevation (DEM) {#terrain} + + + +By default SIH simulates flat ground at [`SIH_LOC_H0`](../advanced_config/parameter_reference.md#SIH_LOC_H0). +SIH can instead load the terrain elevation from a digital elevation model (DEM) [GeoTIFF](https://www.ogc.org/standards/geotiff/) file, set with the `PX4_SIH_TERRAIN` environment variable: + +```sh +PX4_SIH_TERRAIN=/path/to/dem.tif make px4_sitl_sih sihsim_quadx +``` + +The terrain is then used for: + +- Ground contact: the vehicle starts, lands and rests on the terrain. + It is placed at the terrain elevation below [`SIH_LOC_LAT0`](../advanced_config/parameter_reference.md#SIH_LOC_LAT0)/[`SIH_LOC_LON0`](../advanced_config/parameter_reference.md#SIH_LOC_LON0) (`PX4_HOME_LAT`/`PX4_HOME_LON`). + `SIH_LOC_H0` (`PX4_HOME_ALT`) sets the start altitude only when that point is outside the DEM. +- The downward facing distance sensor, which measures the range to the terrain along the vehicle body z axis (up to [`SIH_DISTSNSR_MAX`](../advanced_config/parameter_reference.md#SIH_DISTSNSR_MAX)). +- The `terrain_alt` field of the `vehicle_global_position_groundtruth` topic. +- The `dist_bottom` field of the `vehicle_local_position_groundtruth` topic. + +Outside the area covered by the DEM, or over cells without data, the ground is flat at `SIH_LOC_H0` and a warning is printed. +Terrain is supported for multicopters, VTOLs and fixed-wing vehicles; with any other vehicle type (such as Ackermann rovers), setting `PX4_SIH_TERRAIN` stops SIH from starting with an error. +`simulator_sih status` shows the loaded DEM and whether the vehicle is inside it. + +The DEM must be a single-band GeoTIFF whose coordinate reference system has an [EPSG code](https://epsg.io/), either geographic (such as WGS84, `EPSG:4326`) or projected (such as UTM or the Swiss `EPSG:2056`). +8, 16 and 32-bit integer and 32 and 64-bit floating point samples are supported with any TIFF compression, and the [_GDAL_](https://gdal.org/) nodata value is respected. +Elevations are used as they are, as altitude above mean sea level. +Free sources include [Copernicus GLO-30](https://registry.opendata.aws/copernicus-dem/) (near-global, 30 m, including buildings and vegetation) and national elevation models such as [swissALTI3D](https://www.swisstopo.admin.ch/en/height-model-swissalti3d) (2 m). +Crop large files to the area you fly in, for example with _GDAL_: + +```sh +gdal_translate -projwin 8.48 47.44 8.61 47.35 Copernicus_DSM_COG_10_N47_00_E008_00_DEM.tif zurich.tif +``` + +Files in other formats, with other sample types or without an EPSG code can be converted with `gdalwarp -t_srs EPSG:4326 -ot Float32 in.tif out.tif`. + +::: info +Terrain support is optional and only available in SITL, not when [running SIH on flight controller hardware](#sih-on-flight-controller-hardware). +The [prebuilt](../simulation/px4_sitl_prebuilt_packages.md) `px4` package and `px4io/px4-sitl` container are built without it, and SIH reports an error and does not start if `PX4_SIH_TERRAIN` is set. +When building from source, the [setup scripts](../dev_setup/dev_env.md) install the required libraries with the simulation tools. +The CMake output shows `SIH terrain (DEM) support enabled` when it is available. +::: + ### ROS 2 Integration SIH works with ROS 2 via the [uXRCE-DDS](../middleware/uxrce_dds.md) client, which auto-starts in SITL mode. diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index 6532bf8b30a..1aa35a77194 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -49,6 +49,33 @@ px4_add_module( geo ) +# optional terrain from a GeoTIFF DEM, SITL only (requires libtiff, libgeotiff and PROJ) +if(${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL} STREQUAL "px4_sitl") + find_package(TIFF QUIET) + find_package(PROJ CONFIG QUIET) + find_path(GEOTIFF_INCLUDE_DIR geotiff.h PATH_SUFFIXES geotiff libgeotiff) + find_library(GEOTIFF_LIBRARY NAMES geotiff geotiff_i) + + if(TIFF_FOUND AND PROJ_FOUND AND GEOTIFF_INCLUDE_DIR AND GEOTIFF_LIBRARY) + message(STATUS "SIH terrain (DEM) support enabled") + + px4_add_library(sih_terrain terrain/TerrainDem.cpp) + target_include_directories(sih_terrain PRIVATE ${GEOTIFF_INCLUDE_DIR}) + target_link_libraries(sih_terrain PRIVATE ${GEOTIFF_LIBRARY} TIFF::TIFF PROJ::proj geo) + + target_link_libraries(modules__simulation__simulator_sih PRIVATE sih_terrain) + target_compile_definitions(modules__simulation__simulator_sih PRIVATE SIH_TERRAIN_DEM) + + px4_add_unit_gtest(SRC terrain/TerrainDemTest.cpp + INCLUDES ${GEOTIFF_INCLUDE_DIR} + LINKLIBS sih_terrain geo ${GEOTIFF_LIBRARY} TIFF::TIFF + ) + + else() + message(STATUS "SIH terrain (DEM) support disabled: libtiff, libgeotiff or PROJ not found") + endif() +endif() + if(PX4_PLATFORM MATCHES "posix") # create targets for sihsim set(models diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index e007fea4f28..4fe56eeef13 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -96,6 +96,7 @@ Sih::~Sih() { perf_free(_loop_perf); perf_free(_loop_interval_perf); + _terrain = Terrain{}; // free the DEM } void Sih::run() @@ -103,6 +104,11 @@ void Sih::run() _px4_accel.set_temperature(T1_C); _px4_gyro.set_temperature(T1_C); + // the vehicle type is needed by parameters_updated() to place the vehicle on the terrain + _vehicle = static_cast(constrain(_sih_vtype.get(), + static_cast(VehicleType::First), + static_cast(VehicleType::Last))); + parameters_updated(); if (_sih_optical_flow_en.get()) { @@ -119,9 +125,6 @@ void Sih::run() _dist_snsr_time = task_start; _optical_flow_time = task_start; _ranging_beacon_time = task_start; - _vehicle = static_cast(constrain(_sih_vtype.get(), - static_cast(VehicleType::First), - static_cast(VehicleType::Last))); #if defined(ENABLE_LOCKSTEP_SCHEDULER) lockstep_loop(); @@ -408,13 +411,19 @@ void Sih::parameters_updated() if (!_lpos_ref.isInitialized() || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLat()) - _sih_lat0.get()) > FLT_EPSILON) || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLon()) - _sih_lon0.get()) > FLT_EPSILON) - || (fabsf(_lpos_ref_alt - _sih_h0.get()) > FLT_EPSILON)) { - _lpos_ref.initReference(static_cast(_sih_lat0.get()), static_cast(_sih_lon0.get())); - _lpos_ref_alt = _sih_h0.get(); + || (fabsf(_terrain.defaultAltitude() - _sih_h0.get()) > FLT_EPSILON)) { + const double lat0 = static_cast(_sih_lat0.get()); + const double lon0 = static_cast(_sih_lon0.get()); + _lpos_ref.initReference(lat0, lon0); + + // start on the ground: flat at SIH_LOC_H0, or on the terrain model + _terrain.setDefaultAltitude(_sih_h0.get()); + _lpos_ref_alt = _terrain.vehicleGroundAltitude(lat0, lon0); + _ground_alt = _lpos_ref_alt; // Reset earth position, velocity and attitude - _lla.setLatitudeDeg(static_cast(_sih_lat0.get())); - _lla.setLongitudeDeg(static_cast(_sih_lon0.get())); + _lla.setLatitudeDeg(lat0); + _lla.setLongitudeDeg(lon0); _lla.setAltitude(_lpos_ref_alt); _p_E = _lla.toEcef(); @@ -755,7 +764,7 @@ void Sih::equations_of_motion(const float dt) const float force_down = Vector3f(_R_N2E.transpose() * sum_of_forces_E)(2); Vector3f ground_force_E; - if ((_lla.altitude() - _lpos_ref_alt) < 0.f && force_down > 0.f) { + if (_lla.altitude() < _ground_alt && force_down > 0.f) { if (_vehicle == VehicleType::Quadcopter || _vehicle == VehicleType::Hexacopter || _vehicle == VehicleType::TailsitterVTOL @@ -815,6 +824,23 @@ void Sih::equations_of_motion(const float dt) ecefToNed(); + // ground below the new position, for the next contact check and the sensors + _ground_alt = _terrain.vehicleGroundAltitude(_lla.latitude_deg(), _lla.longitude_deg()); + + // the ground force only stops the vertical motion, keep the vehicle from sinking into rising terrain + static constexpr float MAX_PENETRATION = 0.05f; // [m] + + if (_lla.altitude() < _ground_alt - MAX_PENETRATION) { + _lla.setAltitude(_ground_alt - MAX_PENETRATION); + _p_E = _lla.toEcef(); + + if (_v_N(2) > 0.f) { + _v_N(2) = 0.f; + _v_E = _R_N2E * _v_N; + _v_apparent_N = _v_N + _v_wind_N; + } + } + _lpos_ref.project(_lla.latitude_deg(), _lla.longitude_deg(), _lpos(0), _lpos(1)); _lpos(2) = -(_lla.altitude() - _lpos_ref_alt); } @@ -933,6 +959,11 @@ void Sih::orient_step(const hrt_abstime &now, const float dt) } #endif // __PX4_POSIX +float Sih::ground_distance(float max_distance) const +{ + return _terrain.rayDistance(_lla.latitude_deg(), _lla.longitude_deg(), _lla.altitude(), _q.dcm_z(), max_distance); +} + void Sih::ecefToNed() { _lla = LatLonAlt::fromEcef(_p_E); @@ -1009,21 +1040,6 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) _airspeed_pub.publish(airspeed); } -float Sih::ground_distance() const -{ - // the ground is assumed flat at the altitude of the simulation reference (SIH_LOC_H0) - const float cos_tilt = _q.dcm_z()(2); - - if (cos_tilt < FLT_EPSILON) { - // the body down axis points away from the ground - return NAN; - } - - // the fake ground lets the vehicle settle marginally below the reference altitude, clamp it - // so that a landed vehicle reads zero instead of dropping out of range - return math::max(-_lpos(2), 0.f) / cos_tilt; -} - void Sih::send_dist_snsr(const hrt_abstime &time_now_us) { if (_distance_sensor_blocked) { @@ -1041,7 +1057,7 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us) } else { // the quality is decided on the true distance rather than on the noisy reading, so // that a measurement sitting at a range limit does not flicker in and out of validity - const float true_distance = ground_distance(); + const float true_distance = ground_distance(_distance_snsr_max); if (!PX4_ISFINITE(true_distance) || true_distance > _distance_snsr_max) { // nothing within reach, the sensor reports its saturated raw reading @@ -1080,7 +1096,7 @@ void Sih::accumulate_optical_flow(const float dt) // value is produced by a right handed rotation of the sensor about the body axis, which // gives the translation over the ground the opposite sign (see the flow observation model // in the EKF). - const float distance = ground_distance(); + const float distance = ground_distance(_sih_optical_flow_h_max.get()); if (!PX4_ISFINITE(distance) || distance > _sih_optical_flow_h_max.get()) { // too high above the ground, or not looking at it: no image to track @@ -1260,6 +1276,8 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) local_position.heading_good_for_control = true; local_position.unaided_heading = NAN; + local_position.dist_bottom = _lla.altitude() - _ground_alt; + local_position.timestamp = hrt_absolute_time(); _local_position_ground_truth_pub.publish(local_position); } @@ -1272,7 +1290,7 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) global_position.lon = _lla.longitude_deg(); global_position.alt = _lla.altitude(); global_position.alt_ellipsoid = global_position.alt; - global_position.terrain_alt = -_lpos(2); + global_position.terrain_alt = _ground_alt; global_position.timestamp = hrt_absolute_time(); _global_position_ground_truth_pub.publish(global_position); } @@ -1313,6 +1331,9 @@ int Sih::print_status() PX4_INFO("Rover Ackermann"); } + _terrain.printStatus(); + + PX4_INFO("ground altitude: %.2f m", (double)_ground_alt); PX4_INFO("vehicle landed: %d", _grounded); #if defined(__PX4_POSIX) @@ -1353,8 +1374,53 @@ int Sih::run_trampoline(int argc, char *argv[]) }, argc, argv); } +bool Sih::load_terrain(int argc, char *argv[]) +{ + _terrain = Terrain{}; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *terrain_path = nullptr; + + while ((ch = px4_getopt(argc, argv, "t:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 't': + terrain_path = myoptarg; + break; + + default: + print_usage("unrecognized flag"); + return false; + } + } + + if (!terrain_path) { + return true; + } + + int32_t vehicle_type = 0; + param_get(param_find("SIH_VEHICLE_TYPE"), &vehicle_type); + const VehicleType vehicle = static_cast(vehicle_type); + + if (vehicle != VehicleType::Quadcopter + && vehicle != VehicleType::Hexacopter + && vehicle != VehicleType::FixedWing + && vehicle != VehicleType::TailsitterVTOL + && vehicle != VehicleType::StandardVTOL) { + PX4_ERR("terrain is not supported for SIH_VEHICLE_TYPE %" PRId32, vehicle_type); + return false; + } + + return _terrain.load(terrain_path); +} + int Sih::task_spawn(int argc, char *argv[]) { + if (!load_terrain(argc, argv)) { + return -1; + } + desc.task_id = px4_task_spawn_cmd("sih", SCHED_DEFAULT, SCHED_PRIORITY_MAX, @@ -1472,6 +1538,7 @@ $ simulator_sih orient off PRINT_MODULE_USAGE_ARG("down|up|front|back|left|right|all|off", "Side facing down, every side in turn, or level", false); PRINT_MODULE_USAGE_ARG("rotate", "Rotate about the vertical", true); #endif // __PX4_POSIX + PRINT_MODULE_USAGE_PARAM_STRING('t', nullptr, "", "Terrain elevation GeoTIFF (DEM), SITL only", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index ca7a2e87a18..76593eaf629 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -92,6 +92,8 @@ #include #endif +#include "terrain/Terrain.hpp" + using namespace time_literals; class Sih : public ModuleBase, public ModuleParams @@ -220,10 +222,6 @@ private: void send_optical_flow(const hrt_abstime &time_now_us); void send_ranging_beacon(const hrt_abstime &time_now_us); - // distance from the vehicle to the ground measured along the body down axis, NAN if the - // body down axis does not point at the ground anymore - float ground_distance() const; - void publish_ground_truth(const hrt_abstime &time_now_us); void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust_for_prowash); void generate_ts_aerodynamics(); @@ -253,6 +251,16 @@ private: float _orient_yaw{0.f}; // heading kept while posing #endif // __PX4_POSIX + // distance from the vehicle to the terrain measured along the body down axis, NAN if the + // body down axis does not point at the ground or nothing is hit within max_distance + float ground_distance(float max_distance) const; + + // terrain from the start arguments: the -t DEM, flat without it + // false (and print the reason) on error or if the vehicle type does not support terrain + static bool load_terrain(int argc, char *argv[]); + + static inline Terrain _terrain{}; // loaded by task_spawn(), the SIH task stack is too small for the terrain libraries + #if defined(ENABLE_LOCKSTEP_SCHEDULER) void lockstep_loop(); int _lockstep_component{-1}; @@ -355,6 +363,7 @@ private: // parameters MapProjection _lpos_ref{}; float _lpos_ref_alt; + float _ground_alt{0.f}; // ground altitude below the vehicle [m AMSL] float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _T_TAU, _F_T_MAX, _F_Q_MAX; matrix::Matrix3f _I; // vehicle inertia matrix matrix::Matrix3f _Im1; // inverse of the inertia matrix diff --git a/src/modules/simulation/simulator_sih/terrain/Terrain.hpp b/src/modules/simulation/simulator_sih/terrain/Terrain.hpp new file mode 100644 index 00000000000..68dc1e0b73a --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/Terrain.hpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Terrain.hpp + * Terrain model used by SIH: the GeoTIFF DEM when PX4 is built with its libraries, flat otherwise. + */ + +#pragma once + +#if defined(SIH_TERRAIN_DEM) +#include "TerrainDem.hpp" +using Terrain = TerrainDem; +#else +#include "TerrainFlat.hpp" +using Terrain = TerrainFlat; +#endif diff --git a/src/modules/simulation/simulator_sih/terrain/TerrainDem.cpp b/src/modules/simulation/simulator_sih/terrain/TerrainDem.cpp new file mode 100644 index 00000000000..9d0dd7b1763 --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/TerrainDem.cpp @@ -0,0 +1,598 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "TerrainDem.hpp" + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace +{ + +constexpr double kMetersPerDegree = 6371000.0 * M_PI / 180.0; +constexpr size_t kMaxCells = 200'000'000; // ~800 MB of floats + +constexpr const char *kConvertHint = + "convert with: gdalwarp -t_srs EPSG:4326 -ot Float32 -co COMPRESS=LZW "; + +TIFFExtendProc parent_tag_extender = nullptr; +const TIFFFieldInfo *custom_field_info = nullptr; // set by TerrainDem::registerTiffTags() +int custom_field_count = 0; + +void tagExtender(TIFF *tif) +{ + TIFFMergeFieldInfo(tif, custom_field_info, custom_field_count); + + if (parent_tag_extender) { + parent_tag_extender(tif); + } +} + +// RAII wrapper so that every early return closes the file +struct GeoTiffFile { + TIFF *tif{nullptr}; + GTIF *gtif{nullptr}; + + ~GeoTiffFile() + { + if (gtif) { + GTIFFree(gtif); + } + + if (tif) { + XTIFFClose(tif); + } + } +}; + +// read one sample of a raster row/tile buffer as float +bool sampleReader(uint16_t bits, uint16_t format, float ( *&read)(const uint8_t *, size_t)) +{ + read = nullptr; + + if (format == SAMPLEFORMAT_UINT) { + if (bits == 8) { + read = [](const uint8_t *p, size_t i) { return static_cast(p[i]); }; + + } else if (bits == 16) { + read = [](const uint8_t *p, size_t i) { uint16_t v; memcpy(&v, p + 2 * i, 2); return static_cast(v); }; + + } else if (bits == 32) { + read = [](const uint8_t *p, size_t i) { uint32_t v; memcpy(&v, p + 4 * i, 4); return static_cast(v); }; + } + + } else if (format == SAMPLEFORMAT_INT) { + if (bits == 8) { + read = [](const uint8_t *p, size_t i) { return static_cast(static_cast(p[i])); }; + + } else if (bits == 16) { + read = [](const uint8_t *p, size_t i) { int16_t v; memcpy(&v, p + 2 * i, 2); return static_cast(v); }; + + } else if (bits == 32) { + read = [](const uint8_t *p, size_t i) { int32_t v; memcpy(&v, p + 4 * i, 4); return static_cast(v); }; + } + + } else if (format == SAMPLEFORMAT_IEEEFP) { + if (bits == 32) { + read = [](const uint8_t *p, size_t i) { float v; memcpy(&v, p + 4 * i, 4); return v; }; + + } else if (bits == 64) { + read = [](const uint8_t *p, size_t i) { double v; memcpy(&v, p + 8 * i, 8); return static_cast(v); }; + } + } + + return read != nullptr; +} + +} // namespace + +void TerrainDem::registerTiffTags() +{ + static const TIFFFieldInfo field_info[] = { + {kTiffTagGdalNodata, -1, -1, TIFF_ASCII, FIELD_CUSTOM, 1, 0, const_cast("GDALNoDataValue")}, + }; + + static bool registered = false; + + if (!registered) { + registered = true; + custom_field_info = field_info; + custom_field_count = sizeof(field_info) / sizeof(field_info[0]); + parent_tag_extender = TIFFSetTagExtender(tagExtender); + } +} + +bool TerrainDem::Grid::sample(double x, double y, float &value) const +{ + const double fx = (x - x0) / dx; + const double fy = (y - y0) / dy; + + // accept up to half a cell outside the outermost cell centres (also rejects NAN) + if (!(fx >= -0.5 && fy >= -0.5 && fx <= width - 0.5 && fy <= height - 0.5)) { + return false; + } + + const double cx = fmin(fmax(fx, 0.), width - 1.); + const double cy = fmin(fmax(fy, 0.), height - 1.); + const int col = std::min(static_cast(cx), width - 2); + const int row = std::min(static_cast(cy), height - 2); + const float tx = static_cast(cx - col); + const float ty = static_cast(cy - row); + + const size_t i = static_cast(row) * width + col; + const float v00 = data[i]; + const float v01 = data[i + 1]; + const float v10 = data[i + width]; + const float v11 = data[i + width + 1]; + + if (std::isnan(v00) || std::isnan(v01) || std::isnan(v10) || std::isnan(v11)) { + return false; + } + + value = (1.f - ty) * ((1.f - tx) * v00 + tx * v01) + ty * ((1.f - tx) * v10 + tx * v11); + return true; +} + +bool TerrainDem::load(const char *path) +{ + registerTiffTags(); + + GeoTiffFile file; + file.tif = XTIFFOpen(path, "r"); + + if (!file.tif) { + PX4_ERR("terrain: cannot open %s", path); + return false; + } + + file.gtif = GTIFNew(file.tif); + + if (!file.gtif) { + PX4_ERR("terrain: %s has no GeoTIFF keys, %s", path, kConvertHint); + return false; + } + + TIFF *tif = file.tif; + GTIF *gtif = file.gtif; + + // raster layout + uint32_t width = 0; + uint32_t height = 0; + uint16_t samples_per_pixel = 1; + uint16_t bits_per_sample = 0; + uint16_t sample_format = SAMPLEFORMAT_UINT; + TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &width); + TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &height); + TIFFGetFieldDefaulted(tif, TIFFTAG_SAMPLESPERPIXEL, &samples_per_pixel); + TIFFGetFieldDefaulted(tif, TIFFTAG_BITSPERSAMPLE, &bits_per_sample); + TIFFGetFieldDefaulted(tif, TIFFTAG_SAMPLEFORMAT, &sample_format); + + if (samples_per_pixel != 1) { + PX4_ERR("terrain: %s has %u bands, a single elevation band is required", path, samples_per_pixel); + return false; + } + + if (width < 2 || height < 2 || static_cast(width) * height > kMaxCells) { + PX4_ERR("terrain: %s is %ux%u cells, it must be between 2x2 and %zu cells (crop or downsample it)", + path, width, height, kMaxCells); + return false; + } + + float (*read_sample)(const uint8_t *, size_t) = nullptr; + + if (!sampleReader(bits_per_sample, sample_format, read_sample)) { + PX4_ERR("terrain: %s has unsupported samples (%u bit, format %u), %s", path, bits_per_sample, sample_format, + kConvertHint); + return false; + } + + // georeferencing: pixel corner (0, 0) in model coordinates and pixel size + Grid src{}; + uint16_t count = 0; + double *values = nullptr; + + if (TIFFGetField(tif, TIFFTAG_GEOTRANSMATRIX, &count, &values) && count >= 16) { + if (fabs(values[1]) > 0. || fabs(values[4]) > 0.) { + PX4_ERR("terrain: %s is rotated, %s", path, kConvertHint); + return false; + } + + src.dx = values[0]; + src.dy = values[5]; + src.x0 = values[3]; + src.y0 = values[7]; + + } else { + double *tiepoint = nullptr; + uint16_t tiepoint_count = 0; + + if (!TIFFGetField(tif, TIFFTAG_GEOPIXELSCALE, &count, &values) || count < 2 + || !TIFFGetField(tif, TIFFTAG_GEOTIEPOINTS, &tiepoint_count, &tiepoint) || tiepoint_count < 6) { + PX4_ERR("terrain: %s has no pixel scale and tiepoint, %s", path, kConvertHint); + return false; + } + + src.dx = values[0]; + src.dy = -values[1]; + src.x0 = tiepoint[3] - tiepoint[0] * src.dx; + src.y0 = tiepoint[4] - tiepoint[1] * src.dy; + } + + unsigned short raster_type = RasterPixelIsArea; + GTIFKeyGet(gtif, GTRasterTypeGeoKey, &raster_type, 0, 1); + + if (raster_type != RasterPixelIsPoint) { + // the model coordinates refer to the pixel corner, move them to the pixel centre + src.x0 += 0.5 * src.dx; + src.y0 += 0.5 * src.dy; + } + + if (!(src.dx > 0.) || !(src.dy < 0.)) { + PX4_ERR("terrain: %s must be north-up with increasing columns towards east, %s", path, kConvertHint); + return false; + } + + // coordinate reference system + unsigned short model_type = 0; + unsigned short epsg = 0; + GTIFKeyGet(gtif, GTModelTypeGeoKey, &model_type, 0, 1); + + if (model_type == ModelTypeGeographic) { + GTIFKeyGet(gtif, GeographicTypeGeoKey, &epsg, 0, 1); + + unsigned short angular_units = Angular_Degree; + GTIFKeyGet(gtif, GeogAngularUnitsGeoKey, &angular_units, 0, 1); + + if (angular_units != Angular_Degree) { + PX4_ERR("terrain: %s geographic units are not degrees, %s", path, kConvertHint); + return false; + } + + } else if (model_type == ModelTypeProjected) { + GTIFKeyGet(gtif, ProjectedCSTypeGeoKey, &epsg, 0, 1); + } + + if (epsg == 0 || epsg == KvUserDefined) { + PX4_ERR("terrain: %s has no EPSG coded CRS, %s", path, kConvertHint); + return false; + } + + // nodata + bool has_nodata = false; + float nodata = 0.f; + char *nodata_str = nullptr; + + if (TIFFGetField(tif, kTiffTagGdalNodata, &nodata_str) && nodata_str) { + char *end = nullptr; + nodata = strtof(nodata_str, &end); + has_nodata = (end != nodata_str) && !std::isnan(nodata); + } + + // samples + src.width = width; + src.height = height; + src.data.resize(static_cast(width) * height); + + const auto store = [&](uint32_t col, uint32_t row, float value) { + if (has_nodata && fabsf(value - nodata) <= FLT_EPSILON * fmaxf(1.f, fabsf(nodata))) { + value = NAN; + } + + src.data[static_cast(row) * width + col] = value; + }; + + if (TIFFIsTiled(tif)) { + uint32_t tile_width = 0; + uint32_t tile_height = 0; + TIFFGetField(tif, TIFFTAG_TILEWIDTH, &tile_width); + TIFFGetField(tif, TIFFTAG_TILELENGTH, &tile_height); + std::vector buffer(TIFFTileSize(tif)); + + for (uint32_t tile_row = 0; tile_row < height; tile_row += tile_height) { + for (uint32_t tile_col = 0; tile_col < width; tile_col += tile_width) { + if (TIFFReadTile(tif, buffer.data(), tile_col, tile_row, 0, 0) < 0) { + PX4_ERR("terrain: %s: failed to read tile at %u,%u", path, tile_col, tile_row); + return false; + } + + for (uint32_t r = 0; r < tile_height && tile_row + r < height; r++) { + for (uint32_t c = 0; c < tile_width && tile_col + c < width; c++) { + store(tile_col + c, tile_row + r, read_sample(buffer.data(), static_cast(r) * tile_width + c)); + } + } + } + } + + } else { + std::vector buffer(TIFFScanlineSize(tif)); + + for (uint32_t row = 0; row < height; row++) { + if (TIFFReadScanline(tif, buffer.data(), row, 0) < 0) { + PX4_ERR("terrain: %s: failed to read row %u", path, row); + return false; + } + + for (uint32_t col = 0; col < width; col++) { + store(col, row, read_sample(buffer.data(), col)); + } + } + } + + _epsg = epsg; + _path = path; + + if (epsg == 4326) { + _grid = std::move(src); + const double lat_center = _grid.y0 + 0.5 * (_grid.height - 1) * _grid.dy; + _cell_size_m = static_cast(kMetersPerDegree * fmin(-_grid.dy, _grid.dx * cos(lat_center * M_PI / 180.))); + + } else if (!resampleToWgs84(src)) { + _grid = Grid{}; + return false; + } + + printStatus(); + + return true; +} + +bool TerrainDem::resampleToWgs84(const Grid &src) +{ + char crs[32]; + snprintf(crs, sizeof(crs), "EPSG:%d", _epsg); + + PJ_CONTEXT *ctx = proj_context_create(); + PJ *wgs84_to_src = nullptr; + + { + PJ *transform = proj_create_crs_to_crs(ctx, "EPSG:4326", crs, nullptr); + + if (transform) { + // lon/lat and easting/northing axis order, whatever the CRS definitions say + wgs84_to_src = proj_normalize_for_visualization(ctx, transform); + proj_destroy(transform); + } + } + + if (!wgs84_to_src) { + PX4_ERR("terrain: PROJ cannot transform WGS84 to %s (%s), %s", crs, + proj_context_errno_string(ctx, proj_context_errno(ctx)), kConvertHint); + proj_context_destroy(ctx); + return false; + } + + // WGS84 bounding box of the source raster, from points along its edges + constexpr int kEdgePoints = 32; + std::vector lon; + std::vector lat; + + for (int i = 0; i <= kEdgePoints; i++) { + const double t = static_cast(i) / kEdgePoints; + const double col = t * (src.width - 1); + const double row = t * (src.height - 1); + const double edge_cols[4] = {col, col, 0., src.width - 1.}; + const double edge_rows[4] = {0., src.height - 1., row, row}; + + for (int edge = 0; edge < 4; edge++) { + lon.push_back(src.x0 + edge_cols[edge] * src.dx); + lat.push_back(src.y0 + edge_rows[edge] * src.dy); + } + } + + // the centre pixel and its neighbours, to keep the source resolution + const double center_col = 0.5 * (src.width - 1); + const double center_row = 0.5 * (src.height - 1); + const size_t center = lon.size(); + lon.insert(lon.end(), {src.x0 + center_col * src.dx, src.x0 + (center_col + 1.) * src.dx, src.x0 + center_col * src.dx}); + lat.insert(lat.end(), {src.y0 + center_row * src.dy, src.y0 + center_row * src.dy, src.y0 + (center_row + 1.) * src.dy}); + + proj_trans_generic(wgs84_to_src, PJ_INV, lon.data(), sizeof(double), lon.size(), lat.data(), sizeof(double), lat.size(), + nullptr, 0, 0, nullptr, 0, 0); + + double lon_min = static_cast(INFINITY); + double lon_max = -static_cast(INFINITY); + double lat_min = static_cast(INFINITY); + double lat_max = -static_cast(INFINITY); + + for (size_t i = 0; i < lon.size(); i++) { + if (!std::isfinite(lon[i]) || !std::isfinite(lat[i])) { + PX4_ERR("terrain: %s extent cannot be transformed to WGS84", _path.c_str()); + proj_destroy(wgs84_to_src); + proj_context_destroy(ctx); + return false; + } + + lon_min = fmin(lon_min, lon[i]); + lon_max = fmax(lon_max, lon[i]); + lat_min = fmin(lat_min, lat[i]); + lat_max = fmax(lat_max, lat[i]); + } + + const double cos_lat = cos(lat[center] * M_PI / 180.); + const double step_col_m = kMetersPerDegree * hypot(lat[center + 1] - lat[center], (lon[center + 1] - lon[center]) * cos_lat); + const double step_row_m = kMetersPerDegree * hypot(lat[center + 2] - lat[center], (lon[center + 2] - lon[center]) * cos_lat); + const double cell_m = fmin(step_col_m, step_row_m); + + Grid dst{}; + dst.dx = cell_m / (kMetersPerDegree * cos_lat); + dst.dy = -cell_m / kMetersPerDegree; + dst.x0 = lon_min; + dst.y0 = lat_max; + dst.width = static_cast((lon_max - lon_min) / dst.dx) + 1; + dst.height = static_cast((lat_max - lat_min) / -dst.dy) + 1; + + if (dst.width < 2 || dst.height < 2 || static_cast(dst.width) * dst.height > kMaxCells) { + PX4_ERR("terrain: %s resampled to WGS84 is %dx%d cells (crop or downsample it)", _path.c_str(), dst.width, dst.height); + proj_destroy(wgs84_to_src); + proj_context_destroy(ctx); + return false; + } + + dst.data.resize(static_cast(dst.width) * dst.height); + + // transform one row of WGS84 cell centres at a time into the source CRS and sample the source there + std::vector x(dst.width); + std::vector y(dst.width); + + for (int row = 0; row < dst.height; row++) { + for (int col = 0; col < dst.width; col++) { + x[col] = dst.x0 + col * dst.dx; + y[col] = dst.y0 + row * dst.dy; + } + + proj_trans_generic(wgs84_to_src, PJ_FWD, x.data(), sizeof(double), x.size(), y.data(), sizeof(double), y.size(), + nullptr, 0, 0, nullptr, 0, 0); + + for (int col = 0; col < dst.width; col++) { + float value; + + if (!src.sample(x[col], y[col], value)) { + value = NAN; + } + + dst.data[static_cast(row) * dst.width + col] = value; + } + } + + proj_destroy(wgs84_to_src); + proj_context_destroy(ctx); + + _grid = std::move(dst); + _cell_size_m = static_cast(cell_m); + return true; +} + +bool TerrainDem::altitude(double lat_deg, double lon_deg, float &alt) const +{ + if (!loaded()) { + return TerrainFlat::altitude(lat_deg, lon_deg, alt); + } + + if (_grid.sample(lon_deg, lat_deg, alt)) { + return true; + } + + alt = _default_alt; + return false; +} + +float TerrainDem::vehicleGroundAltitude(double lat_deg, double lon_deg) +{ + float alt; + const bool inside = altitude(lat_deg, lon_deg, alt); + + if (!inside && !_vehicle_outside) { + PX4_WARN("terrain: vehicle outside the DEM, using flat ground at %.1f m", (double)_default_alt); + + } else if (inside && _vehicle_outside) { + PX4_INFO("terrain: vehicle back inside the DEM"); + } + + _vehicle_outside = !inside; + return alt; +} + +float TerrainDem::rayDistance(double lat_deg, double lon_deg, float alt, const matrix::Vector3f &dir_ned, + float max_distance) const +{ + if (!loaded()) { + return TerrainFlat::rayDistance(lat_deg, lon_deg, alt, dir_ned, max_distance); + } + + if (dir_ned(2) <= 0.f) { + return NAN; + } + + float ground_alt; + altitude(lat_deg, lon_deg, ground_alt); + float prev_height = alt - ground_alt; // height above the terrain along the ray + + if (prev_height <= 0.f) { + return 0.f; + } + + MapProjection ref{lat_deg, lon_deg}; + + // sample each DEM cell at least twice, the bilinear terrain is then followed within centimetres + static constexpr float STEPS_PER_CELL = 2.f; + static constexpr float MAX_STEP = 1.f; // [m] + const float step = std::min(_cell_size_m / STEPS_PER_CELL, MAX_STEP); + float prev_distance = 0.f; + + for (float distance = step; distance < max_distance + step; distance += step) { + const matrix::Vector3f point = dir_ned * distance; + double lat; + double lon; + ref.reproject(point(0), point(1), lat, lon); + altitude(lat, lon, ground_alt); + const float height = (alt - point(2)) - ground_alt; + + if (height <= 0.f) { + // interpolate the crossing between the last two samples + return prev_distance + (distance - prev_distance) * prev_height / (prev_height - height); + } + + prev_distance = distance; + prev_height = height; + } + + return NAN; +} + +void TerrainDem::printStatus() const +{ + if (!loaded()) { + TerrainFlat::printStatus(); + return; + } + + PX4_INFO("terrain: %s", path()); + PX4_INFO("terrain: EPSG:%d, %dx%d cells of %.1f m", epsg(), width(), height(), (double)_cell_size_m); + PX4_INFO("terrain: lat [%.5f, %.5f] lon [%.5f, %.5f]", latMin(), latMax(), lonMin(), lonMax()); + + if (_vehicle_outside) { + PX4_INFO("terrain: vehicle outside the DEM"); + } +} diff --git a/src/modules/simulation/simulator_sih/terrain/TerrainDem.hpp b/src/modules/simulation/simulator_sih/terrain/TerrainDem.hpp new file mode 100644 index 00000000000..eb15399f3f9 --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/TerrainDem.hpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file TerrainDem.hpp + * Terrain elevation model loaded from a GeoTIFF DEM, for SIH in SITL. + * + * The raster is read with libtiff/libgeotiff. A DEM in any CRS with an EPSG code is + * resampled once at load time with PROJ onto a regular WGS84 lat/lon grid, so that + * altitude() is a cheap bilinear lookup in the simulation loop. Until a DEM is loaded the terrain is flat. + */ + +#pragma once + +#include "TerrainFlat.hpp" + +#include +#include + +class TerrainDem : public TerrainFlat +{ +public: + TerrainDem() = default; + + /** load a single-band GeoTIFF DEM, check loaded() for the result (the reason of a failure is printed) */ + explicit TerrainDem(const char *path) { load(path); } + + /** load a single-band GeoTIFF DEM, @return false (the reason is printed) on error */ + bool load(const char *path) override; + + bool loaded() const { return !_grid.data.empty(); } + + /** DEM elevation interpolated bilinearly, the default altitude outside the DEM or next to a nodata cell */ + bool altitude(double lat_deg, double lon_deg, float &alt) const override; + + /** the DEM elevation, warns when the vehicle leaves the DEM */ + float vehicleGroundAltitude(double lat_deg, double lon_deg) override; + + /** march along the ray, sampling each DEM cell at least twice */ + float rayDistance(double lat_deg, double lon_deg, float alt, const matrix::Vector3f &dir_ned, + float max_distance) const override; + + void printStatus() const override; + +private: + friend class TerrainDemTest; // unit test, writes GeoTIFF files and checks the parsed metadata + + /** smallest grid spacing [m] */ + float cellSizeM() const { return _cell_size_m; } + + const char *path() const { return _path.c_str(); } + int epsg() const { return _epsg; } + int width() const { return _grid.width; } + int height() const { return _grid.height; } + + double latMin() const { return _grid.y0 + (_grid.height - 1) * _grid.dy; } + double latMax() const { return _grid.y0; } + double lonMin() const { return _grid.x0; } + double lonMax() const { return _grid.x0 + (_grid.width - 1) * _grid.dx; } + + /** register the GDAL_NODATA TIFF tag with libtiff */ + static void registerTiffTags(); + + static constexpr unsigned kTiffTagGdalNodata = 42113; + + // regular raster, value of cell (col, row) at x = x0 + col * dx, y = y0 + row * dy + struct Grid { + std::vector data; // row-major, NAN for nodata + int width{0}; + int height{0}; + double x0{0.}; + double y0{0.}; + double dx{0.}; + double dy{0.}; + + bool sample(double x, double y, float &value) const; + }; + + bool resampleToWgs84(const Grid &src); + + Grid _grid{}; // x: longitude [deg], y: latitude [deg], dy < 0 (row 0 is the northern edge) + float _cell_size_m{0.f}; + bool _vehicle_outside{false}; + int _epsg{0}; + std::string _path; +}; diff --git a/src/modules/simulation/simulator_sih/terrain/TerrainDemTest.cpp b/src/modules/simulation/simulator_sih/terrain/TerrainDemTest.cpp new file mode 100644 index 00000000000..8f91272ab91 --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/TerrainDemTest.cpp @@ -0,0 +1,394 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#define MODULE_NAME "TerrainDemTest" + +#include + +#include "TerrainDem.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// TerrainDem logs with PX4_INFO/PX4_WARN/PX4_ERR, satisfying them from px4_platform drags uORB and +// the POSIX daemon into this host unit test +__BEGIN_DECLS + +void px4_log_info(const char *moduleName, const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + vfprintf(stdout, fmt, args); + va_end(args); + fputc('\n', stdout); +} + +void px4_log_warn(const char *moduleName, const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); + fputc('\n', stderr); +} + +void px4_log_error(const char *moduleName, const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); + fputc('\n', stderr); +} + +__END_DECLS + +// friend of TerrainDem, gives the tests access to its private metadata +class TerrainDemTest : public testing::Test +{ +public: + static void registerTiffTags() { TerrainDem::registerTiffTags(); } + static constexpr unsigned kTiffTagGdalNodata = TerrainDem::kTiffTagGdalNodata; + + static int epsg(const TerrainDem &dem) { return dem.epsg(); } + static int width(const TerrainDem &dem) { return dem.width(); } + static int height(const TerrainDem &dem) { return dem.height(); } + static float cellSizeM(const TerrainDem &dem) { return dem.cellSizeM(); } +}; + +namespace +{ + +struct TiffSpec { + uint32_t width{3}; + uint32_t height{3}; + unsigned short model_type{ModelTypeGeographic}; + unsigned short epsg{4326}; + double corner_x{8.0}; // model coordinates of the corner of pixel (0, 0) + double corner_y{47.0}; + double scale_x{0.01}; + double scale_y{0.01}; + bool pixel_is_point{false}; + uint16_t bits{32}; + uint16_t format{SAMPLEFORMAT_IEEEFP}; + uint16_t compression{COMPRESSION_NONE}; + uint32_t tile{0}; // 0: strips + uint16_t bands{1}; + const char *nodata{nullptr}; +}; + +using ValueFn = std::function; + +void writeSample(uint8_t *buffer, size_t index, const TiffSpec &spec, double value) +{ + if (spec.format == SAMPLEFORMAT_IEEEFP) { + const float v = static_cast(value); + memcpy(buffer + 4 * index, &v, 4); + + } else { + const int16_t v = static_cast(lround(value)); + memcpy(buffer + 2 * index, &v, 2); + } +} + +std::string writeTiff(const std::string &name, const TiffSpec &spec, const ValueFn &value) +{ + TerrainDemTest::registerTiffTags(); + + const std::string path = testing::TempDir() + name; + TIFF *tif = XTIFFOpen(path.c_str(), "w"); + + if (!tif) { + ADD_FAILURE() << "cannot create " << path; + return path; + } + + TIFFSetField(tif, TIFFTAG_IMAGEWIDTH, spec.width); + TIFFSetField(tif, TIFFTAG_IMAGELENGTH, spec.height); + TIFFSetField(tif, TIFFTAG_SAMPLESPERPIXEL, spec.bands); + TIFFSetField(tif, TIFFTAG_BITSPERSAMPLE, spec.bits); + TIFFSetField(tif, TIFFTAG_SAMPLEFORMAT, spec.format); + TIFFSetField(tif, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG); + TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK); + TIFFSetField(tif, TIFFTAG_COMPRESSION, spec.compression); + + if (spec.tile) { + TIFFSetField(tif, TIFFTAG_TILEWIDTH, spec.tile); + TIFFSetField(tif, TIFFTAG_TILELENGTH, spec.tile); + + } else { + TIFFSetField(tif, TIFFTAG_ROWSPERSTRIP, 1); + } + + if (spec.nodata) { + TIFFSetField(tif, TerrainDemTest::kTiffTagGdalNodata, spec.nodata); + } + + double tiepoint[6] = {0., 0., 0., spec.corner_x, spec.corner_y, 0.}; + double scale[3] = {spec.scale_x, spec.scale_y, 0.}; + TIFFSetField(tif, TIFFTAG_GEOTIEPOINTS, 6, tiepoint); + TIFFSetField(tif, TIFFTAG_GEOPIXELSCALE, 3, scale); + + GTIF *gtif = GTIFNew(tif); + GTIFKeySet(gtif, GTModelTypeGeoKey, TYPE_SHORT, 1, spec.model_type); + GTIFKeySet(gtif, GTRasterTypeGeoKey, TYPE_SHORT, 1, spec.pixel_is_point ? RasterPixelIsPoint : RasterPixelIsArea); + GTIFKeySet(gtif, spec.model_type == ModelTypeGeographic ? GeographicTypeGeoKey : ProjectedCSTypeGeoKey, + TYPE_SHORT, 1, spec.epsg); + GTIFWriteKeys(gtif); + GTIFFree(gtif); + + const size_t sample_size = spec.bits / 8; + + if (spec.tile) { + std::vector buffer(spec.tile * spec.tile * spec.bands * sample_size); + + for (uint32_t tile_row = 0; tile_row < spec.height; tile_row += spec.tile) { + for (uint32_t tile_col = 0; tile_col < spec.width; tile_col += spec.tile) { + std::fill(buffer.begin(), buffer.end(), 0); + + for (uint32_t r = 0; r < spec.tile && tile_row + r < spec.height; r++) { + for (uint32_t c = 0; c < spec.tile && tile_col + c < spec.width; c++) { + for (uint16_t b = 0; b < spec.bands; b++) { + writeSample(buffer.data(), (r * spec.tile + c) * spec.bands + b, spec, value(tile_col + c, tile_row + r)); + } + } + } + + TIFFWriteTile(tif, buffer.data(), tile_col, tile_row, 0, 0); + } + } + + } else { + std::vector buffer(spec.width * spec.bands * sample_size); + + for (uint32_t row = 0; row < spec.height; row++) { + for (uint32_t col = 0; col < spec.width; col++) { + for (uint16_t b = 0; b < spec.bands; b++) { + writeSample(buffer.data(), col * spec.bands + b, spec, value(col, row)); + } + } + + TIFFWriteScanline(tif, buffer.data(), row, 0); + } + } + + XTIFFClose(tif); + return path; +} + +// WGS84 position of the centre of pixel (col, row) of a geographic PixelIsArea raster +double pixelLon(const TiffSpec &spec, double col) { return spec.corner_x + (col + 0.5) * spec.scale_x; } +double pixelLat(const TiffSpec &spec, double row) { return spec.corner_y - (row + 0.5) * spec.scale_y; } + +double ramp(uint32_t col, uint32_t row) { return 400. + 10. * col + 100. * row; } + +} // namespace + +TEST_F(TerrainDemTest, MissingFile) +{ + TerrainDem dem("/nonexistent/dem.tif"); + EXPECT_FALSE(dem.loaded()); +} + +TEST_F(TerrainDemTest, GeographicFloat32) +{ + const TiffSpec spec{}; + TerrainDem dem(writeTiff("float32.tif", spec, ramp).c_str()); + ASSERT_TRUE(dem.loaded()); + EXPECT_EQ(epsg(dem), 4326); + EXPECT_EQ(width(dem), 3); + EXPECT_EQ(height(dem), 3); + + float alt = 0.f; + + // cell centres + for (uint32_t row = 0; row < spec.height; row++) { + for (uint32_t col = 0; col < spec.width; col++) { + ASSERT_TRUE(dem.altitude(pixelLat(spec, row), pixelLon(spec, col), alt)); + EXPECT_FLOAT_EQ(alt, ramp(col, row)); + } + } + + // bilinear between cell centres + ASSERT_TRUE(dem.altitude(pixelLat(spec, 0.5), pixelLon(spec, 1.25), alt)); + EXPECT_NEAR(alt, 400. + 12.5 + 50., 1e-3); + + // within the outer half cell the edge value is used + ASSERT_TRUE(dem.altitude(spec.corner_y - 0.001, spec.corner_x + 0.001, alt)); + EXPECT_FLOAT_EQ(alt, ramp(0, 0)); + + // outside + EXPECT_FALSE(dem.altitude(spec.corner_y + 0.001, pixelLon(spec, 1), alt)); + EXPECT_FALSE(dem.altitude(pixelLat(spec, 1), spec.corner_x + 0.031, alt)); + EXPECT_FALSE(dem.altitude(NAN, NAN, alt)); + + EXPECT_NEAR(cellSizeM(dem), 0.01 * 6371000. * M_PI / 180. * cos(46.985 * M_PI / 180.), 1.); +} + +TEST_F(TerrainDemTest, VehicleGroundAltitude) +{ + const TiffSpec spec{}; + TerrainDem dem; + dem.setDefaultAltitude(123.f); + + // flat until a DEM is loaded + EXPECT_FLOAT_EQ(dem.vehicleGroundAltitude(pixelLat(spec, 1), pixelLon(spec, 1)), 123.f); + + ASSERT_TRUE(dem.load(writeTiff("vehicle.tif", spec, ramp).c_str())); + EXPECT_FLOAT_EQ(dem.vehicleGroundAltitude(pixelLat(spec, 1), pixelLon(spec, 1)), ramp(1, 1)); + + // outside the DEM the ground is flat at the default altitude, and back on the DEM when reentering + EXPECT_FLOAT_EQ(dem.vehicleGroundAltitude(spec.corner_y + 0.001, pixelLon(spec, 1)), 123.f); + EXPECT_FLOAT_EQ(dem.vehicleGroundAltitude(pixelLat(spec, 2), pixelLon(spec, 0)), ramp(0, 2)); +} + +TEST_F(TerrainDemTest, Int16LzwTiled) +{ + TiffSpec spec{}; + spec.width = 21; // partial tiles + spec.height = 19; + spec.bits = 16; + spec.format = SAMPLEFORMAT_INT; + spec.compression = COMPRESSION_LZW; + spec.tile = 16; + + TerrainDem dem(writeTiff("int16_lzw_tiled.tif", spec, ramp).c_str()); + ASSERT_TRUE(dem.loaded()); + + float alt = 0.f; + const uint32_t points[][2] = {{0, 0}, {15, 15}, {16, 16}, {20, 18}, {17, 3}}; + + for (const auto &p : points) { + ASSERT_TRUE(dem.altitude(pixelLat(spec, p[1]), pixelLon(spec, p[0]), alt)); + EXPECT_FLOAT_EQ(alt, ramp(p[0], p[1])); + } + + ASSERT_TRUE(dem.altitude(pixelLat(spec, 15.5), pixelLon(spec, 15.5), alt)); + EXPECT_NEAR(alt, 400. + 155. + 1550., 1e-2); +} + +TEST_F(TerrainDemTest, PixelIsPoint) +{ + TiffSpec spec{}; + spec.pixel_is_point = true; + + TerrainDem dem(writeTiff("pixel_is_point.tif", spec, ramp).c_str()); + ASSERT_TRUE(dem.loaded()); + + // the tiepoint is the centre of pixel (0, 0) + float alt = 0.f; + ASSERT_TRUE(dem.altitude(spec.corner_y, spec.corner_x, alt)); + EXPECT_FLOAT_EQ(alt, ramp(0, 0)); + ASSERT_TRUE(dem.altitude(spec.corner_y - 2. * spec.scale_y, spec.corner_x + 2. * spec.scale_x, alt)); + EXPECT_FLOAT_EQ(alt, ramp(2, 2)); +} + +TEST_F(TerrainDemTest, Nodata) +{ + TiffSpec spec{}; + spec.width = 4; + spec.nodata = "-9999"; + + TerrainDem dem(writeTiff("nodata.tif", spec, [](uint32_t col, uint32_t row) { + return (col == 3 && row == 2) ? -9999. : ramp(col, row); + }).c_str()); + ASSERT_TRUE(dem.loaded()); + + float alt = 0.f; + EXPECT_TRUE(dem.altitude(pixelLat(spec, 0.5), pixelLon(spec, 0.5), alt)); + EXPECT_TRUE(dem.altitude(pixelLat(spec, 1), pixelLon(spec, 2), alt)); + // interpolating with the nodata cell + EXPECT_FALSE(dem.altitude(pixelLat(spec, 1.5), pixelLon(spec, 2.5), alt)); + EXPECT_FALSE(dem.altitude(pixelLat(spec, 2), pixelLon(spec, 3), alt)); +} + +TEST_F(TerrainDemTest, RejectsMultipleBands) +{ + TiffSpec spec{}; + spec.bands = 3; + + TerrainDem dem(writeTiff("rgb.tif", spec, ramp).c_str()); + EXPECT_FALSE(dem.loaded()); +} + +TEST_F(TerrainDemTest, RejectsUserDefinedCrs) +{ + TiffSpec spec{}; + spec.epsg = KvUserDefined; + + TerrainDem dem(writeTiff("user_defined.tif", spec, ramp).c_str()); + EXPECT_FALSE(dem.loaded()); +} + +TEST_F(TerrainDemTest, ProjectedUtm) +{ + // 1 km x 1 km at 10 m in UTM zone 32N (EPSG:32632) around Zurich, a plane rising towards north-east + TiffSpec spec{}; + spec.width = 100; + spec.height = 100; + spec.model_type = ModelTypeProjected; + spec.epsg = 32632; + spec.corner_x = 465000.; + spec.corner_y = 5250000.; + spec.scale_x = 10.; + spec.scale_y = 10.; + + const auto plane = [](double easting, double northing) { + return 500. + 0.1 * (easting - 465000.) + 0.05 * (northing - 5249000.); + }; + + TerrainDem dem(writeTiff("utm.tif", spec, [&](uint32_t col, uint32_t row) { + return plane(spec.corner_x + (col + 0.5) * spec.scale_x, spec.corner_y - (row + 0.5) * spec.scale_y); + }).c_str()); + ASSERT_TRUE(dem.loaded()); + + EXPECT_EQ(epsg(dem), 32632); + EXPECT_NEAR(cellSizeM(dem), 10.f, 0.5f); + + // UTM 32N E 465500 N 5249500 (centre of the raster) is 47.3980425 N, 8.5427984 E (cs2cs EPSG:32632 EPSG:4326) + float alt = 0.f; + ASSERT_TRUE(dem.altitude(47.3980425, 8.5427984, alt)); + EXPECT_NEAR(alt, plane(465500., 5249500.), 0.5); + + // outside the raster + EXPECT_FALSE(dem.altitude(47.40, 8.70, alt)); +} diff --git a/src/modules/simulation/simulator_sih/terrain/TerrainFlat.hpp b/src/modules/simulation/simulator_sih/terrain/TerrainFlat.hpp new file mode 100644 index 00000000000..fc0820481b4 --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/TerrainFlat.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file TerrainFlat.hpp + * Flat terrain at the default altitude, the base of every SIH terrain model. + */ + +#pragma once + +#include +#include + +#include +#include + +class TerrainFlat +{ +public: + virtual ~TerrainFlat() = default; + + /** load terrain data from a file, flat terrain has none, @return false (the reason is printed) */ + virtual bool load(const char *path) + { + PX4_ERR("terrain: cannot load %s, PX4 was built without terrain DEM support", path); + return false; + } + + /** flat ground altitude [m AMSL], also used where the terrain model has no data */ + void setDefaultAltitude(float alt) { _default_alt = alt; } + float defaultAltitude() const { return _default_alt; } + + /** + * Terrain altitude [m AMSL] at a WGS84 position. + * @return false outside the terrain data, alt is then the default altitude + */ + virtual bool altitude(double lat_deg, double lon_deg, float &alt) const + { + alt = _default_alt; + return true; + } + + /** + * Terrain altitude [m AMSL] below the vehicle at a WGS84 position, unlike altitude() it + * keeps track of the vehicle leaving and reentering the terrain data. + */ + virtual float vehicleGroundAltitude(double lat_deg, double lon_deg) + { + return _default_alt; + } + + /** + * Distance [m] from a WGS84 position along a direction to the terrain. + * @param alt altitude of the start point [m AMSL] + * @param dir_ned unit vector in the local NED frame + * @param max_distance search range [m], a result above it may be returned + * @return NAN if the ray does not hit the terrain within max_distance + */ + virtual float rayDistance(double lat_deg, double lon_deg, float alt, const matrix::Vector3f &dir_ned, + float max_distance) const + { + const float cos_tilt = dir_ned(2); + + if (cos_tilt < FLT_EPSILON) { + // pointing away from the ground + return NAN; + } + + // the fake ground lets the vehicle settle marginally below the ground altitude, clamp it + // so that a landed vehicle reads zero instead of dropping out of range + const float height = alt - _default_alt; + return (height > 0.f ? height : 0.f) / cos_tilt; + } + + virtual void printStatus() const + { + PX4_INFO("terrain: flat at %.2f m", (double)_default_alt); + } + +protected: + float _default_alt{0.f}; +};