diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index b83bc92b..174c55c0 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -28,7 +28,7 @@ jobs: - name: build and test uses: ros-tooling/action-ros-ci@0.4.8 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_recovery easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_diagnostic_recovery easynav_fusion_localizer easynav_mhamcl_localizer easynav_mpc_controller easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_octomap_maps_manager easynav_regulated_pp_controller easynav_routes_maps_manager easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_simple_recovery easynav_vff_controller + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_recovery easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_bigmap_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_diagnostic_recovery easynav_fusion_localizer easynav_mhamcl_localizer easynav_mpc_controller easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_octomap_maps_manager easynav_regulated_pp_controller easynav_routes_maps_manager easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_simple_recovery easynav_vff_controller target-ros2-distro: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty_rolling.repos skip-tests: false diff --git a/README.md b/README.md index e141c8b0..e5a84f9d 100644 --- a/README.md +++ b/README.md @@ -84,6 +84,7 @@ Map management plugins that provide, update, and store different environment rep |---|---|---| | `easynav_navmap_maps_manager` | Manages NavMap mesh layers. | [README](./maps_managers/easynav_navmap_maps_manager/README.md) | | `easynav_bonxai_maps_manager` | Manages Bonxai probabilistic voxel maps. | [README](./maps_managers/easynav_bonxai_maps_manager/README.md) | +| `easynav_bigmap_maps_manager` | Large maps: a NavMap whose triangles are fine around the robot and coarse far away (dynamic level of detail of a DEM or of a NavMap with several floors). | [README](./maps_managers/easynav_bigmap_maps_manager/README.md) | | `easynav_octomap_maps_manager` | Manages OctoMap 3D occupancy trees. | [README](./maps_managers/easynav_octomap_maps_manager/README.md) | | `easynav_costmap_maps_manager` | Manages Costmap2D layers with filters. | [README](./maps_managers/easynav_costmap_maps_manager/README.md) | | `easynav_simple_maps_manager` | Minimal example map manager (SimpleMap). | [README](./maps_managers/easynav_simple_maps_manager/README.md) | diff --git a/maps_managers/easynav_bigmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_bigmap_maps_manager/CMakeLists.txt new file mode 100644 index 00000000..7979ec5e --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/CMakeLists.txt @@ -0,0 +1,154 @@ +cmake_minimum_required(VERSION 3.8) +project(easynav_bigmap_maps_manager) + +# Without a build type colcon compiles with no optimisation: the NavMap geometry rebuild of every +# re-mesh took 470 ms instead of 7 ms (measured on the campus map). Release unless told otherwise. +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(pluginlib REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_sensors REQUIRED) +find_package(navmap_core REQUIRED) +find_package(easynav_navmap_maps_manager REQUIRED) +find_package(navmap_ros REQUIRED) +find_package(navmap_ros_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) + +# Pure C++ LOD engine (RTIN error map + distance-driven extraction); ROS-free, unit-testable +add_library(${PROJECT_NAME}_lod SHARED + src/dem_grid.cpp + src/rtin_error_map.cpp + src/lod_policy.cpp + src/narrow_passages.cpp + src/lod_mesher.cpp + src/known_map.cpp + src/navmap_dem.cpp + src/surface_lod.cpp + src/dynamic_layer.cpp + src/obstacle_memory.cpp + src/pipeline_check.cpp +) +target_include_directories(${PROJECT_NAME}_lod PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME}_lod PUBLIC + navmap_core::navmap_core + Eigen3::Eigen +) + +# The EasyNav plugin (loaded by maps_manager_node through pluginlib) +add_library(${PROJECT_NAME} SHARED + src/BigMapMapsManager.cpp + src/navmap_filter_base.cpp + src/marker_publisher.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +# Headers of the NavMap filters only; do NOT link that library (see src/navmap_filter_base.cpp) +get_target_property(NAVMAP_FILTERS_INCLUDES + easynav_navmap_maps_manager::easynav_navmap_maps_manager INTERFACE_INCLUDE_DIRECTORIES) +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${NAVMAP_FILTERS_INCLUDES}) +target_link_libraries(${PROJECT_NAME} PUBLIC + ${PROJECT_NAME}_lod + easynav_common::easynav_common + easynav_core::easynav_core + easynav_sensors::easynav_sensors + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 + navmap_ros::navmap_ros + ${navmap_ros_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_msgs_TARGETS} + ${std_srvs_TARGETS} + ${visualization_msgs_TARGETS} +) + +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(TARGETS ${PROJECT_NAME}_lod ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_lod_mesher test/test_lod_mesher.cpp) + target_link_libraries(test_lod_mesher ${PROJECT_NAME}_lod) + ament_add_gtest(test_lod_closure test/test_lod_closure.cpp TIMEOUT 600) + target_link_libraries(test_lod_closure ${PROJECT_NAME}_lod) + # the DEM file: also written by navmap_tools' navmap_gis_tool (a golden file keeps both honest) + ament_add_gtest(test_dem_grid test/test_dem_grid.cpp) + target_link_libraries(test_dem_grid ${PROJECT_NAME}_lod) + target_compile_definitions(test_dem_grid PRIVATE + BIGMAP_TEST_DATA="${CMAKE_CURRENT_SOURCE_DIR}/test/data") + ament_add_gtest(test_main_piece test/test_main_piece.cpp) + target_link_libraries(test_main_piece ${PROJECT_NAME}_lod) + ament_add_gtest(test_navmap_dem test/test_navmap_dem.cpp) + target_link_libraries(test_navmap_dem ${PROJECT_NAME}_lod) + ament_add_gtest(test_update_watchdog test/test_update_watchdog.cpp) + target_include_directories(test_update_watchdog PRIVATE include) + ament_add_gtest(test_pipeline_check test/test_pipeline_check.cpp) + target_link_libraries(test_pipeline_check ${PROJECT_NAME}_lod) + ament_add_gtest(test_obstacle_memory test/test_obstacle_memory.cpp) + target_link_libraries(test_obstacle_memory ${PROJECT_NAME}_lod) + ament_add_gtest(test_surface_lod test/test_surface_lod.cpp) + target_link_libraries(test_surface_lod ${PROJECT_NAME}_lod) + # the plugin itself: parameters, initialize(), update() with a pose and sensor points + ament_add_gtest(test_bigmap_plugin test/test_bigmap_plugin.cpp TIMEOUT 120) + target_link_libraries(test_bigmap_plugin ${PROJECT_NAME}) + ament_add_gtest(test_garage_lod test/test_garage_lod.cpp) + target_link_libraries(test_garage_lod ${PROJECT_NAME}_lod navmap_ros::navmap_ros) + # the garage map is bigmap_bringup's: next to this package in the BigMap repo, or there from + # easynav_plugins; the test also reads BIGMAP_GARAGE_NAVMAP from the environment + set(BIGMAP_GARAGE_NAVMAP_FILE "") + foreach(candidate + "${CMAKE_CURRENT_SOURCE_DIR}/../bigmap_bringup/maps/parking_garage.navmap" + "${CMAKE_CURRENT_SOURCE_DIR}/../../../BigMap/bigmap_bringup/maps/parking_garage.navmap") + if(EXISTS "${candidate}" AND NOT BIGMAP_GARAGE_NAVMAP_FILE) + set(BIGMAP_GARAGE_NAVMAP_FILE "${candidate}") + endif() + endforeach() + target_compile_definitions(test_garage_lod PRIVATE + BIGMAP_GARAGE_NAVMAP="${BIGMAP_GARAGE_NAVMAP_FILE}") + ament_add_gtest(test_real_maps test/test_real_maps.cpp) + target_link_libraries(test_real_maps ${PROJECT_NAME}_lod navmap_ros::navmap_ros) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}_lod ${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) +# every package whose targets the exported library links (a consumer's find_package() fails on +# "target not found" otherwise) +ament_export_dependencies(easynav_common easynav_core easynav_sensors easynav_navmap_maps_manager pluginlib rclcpp rclcpp_lifecycle + navmap_core navmap_ros navmap_ros_interfaces std_msgs geometry_msgs nav_msgs std_srvs + visualization_msgs tf2 Eigen3) + +# Register the plugin +pluginlib_export_plugin_description_file(easynav_core easynav_bigmap_maps_manager_plugins.xml) + +ament_package() diff --git a/maps_managers/easynav_bigmap_maps_manager/README.md b/maps_managers/easynav_bigmap_maps_manager/README.md new file mode 100644 index 00000000..662d27bc --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/README.md @@ -0,0 +1,287 @@ +# easynav_bigmap_maps_manager + +An [EasyNav](https://github.com/EasyNavigation) **maps manager plugin** (`easynav_bigmap_maps_manager/BigMapMapsManager`, +parameter namespace `bigmap`) for maps too big to plan on at full resolution. It holds one NavMap +that is **fine around the robot and coarse far away**, re-meshes it as the robot moves, and puts +the obstacles the sensors see on it. Planner and controller plug-ins read it through the NavState +(`map.navmap`) like any other NavMap. + +What it gives the rest of the stack: + +- a NavMap (layers `occupancy`, `obstacles`, `inflated_obstacles`, `obstacle_hits`, `elevation`, + `lod_size`, `traversed`, vertex colours) published on `/bigmap/map` and set in the NavState; +- NavCels no bigger than needed: `lengths.cell_size.min` near the robot, up to `cell_size.max` far + away, and always fine over ramps, narrow passages and the ground the robot has driven over; +- obstacles from the sensors with a memory (`obstacle_hits`), and the NavMap filters of + `easynav_navmap_maps_manager` (inflation...) applied on top; +- services to save the map and to forget the obstacles (see "Run time"). + +Two engines, chosen by `multilevel.enabled`: the **grid engine** (outdoor terrain, one height per +x, y: a height grid meshed with RTIN) and the **hierarchy engine** (buildings with floors over each +other: the NavMap's own triangles merged by clustering). The details are in the +`bigmap_bringup` README ("How the NavMap is reduced"), until they move here. + +## Use it + +In the parameter file of EasyNav, next to the other nodes: + +```yaml +maps_manager_node: + ros__parameters: + map_types: [bigmap] + bigmap: + plugin: easynav_bigmap_maps_manager/BigMapMapsManager + terrain: + initial_navmap: maps/my_map.navmap # where the map comes from: see the next section +``` + +Everything else has a default. Two complete, documented examples are installed with the package +(`share/easynav_bigmap_maps_manager/config/`, and in `config/` here): + +| File | For | Map from | Localization | +|---|---|---|---| +| [config/example_indoor.yaml](config/example_indoor.yaml) | a building, several floors and ramps, real robot | a NavMap file (`terrain.initial_navmap`) | external (anything that gives `map -> odom`) | +| [config/example_outdoor.yaml](config/example_outdoor.yaml) | open terrain with GPS (the simulation's file) | a DEM downloaded once (`terrain.dem_file`) | GPS + IMU + odometry (UKF) | + +Each file is a whole EasyNav configuration (system, sensors, localizer, controller, planner, maps +manager); the part of this plugin is `maps_manager_node.ros__parameters.bigmap`, in numbered +sections (2.1 terrain, 2.2 lengths, 2.3 lod, 2.4 traversed, 2.5 obstacles, 2.6 dynamic_map, +2.7 filters, 2.8 output). The header of each file has the table of lengths and says which node +reads what. Run EasyNav with one: + +```bash +ros2 run easynav_system system_main --ros-args --params-file +``` + +The examples are copies of `bigmap_bringup/config/params_indoor.yaml` and `params_outdoor.yaml`, +which the simulations use (a test keeps them equal). + +## Where the map comes from + +The plugin starts from **one** of these, in this order of precedence (`terrain.*`): + +1. `terrain.initial_navmap`: a NavMap file. Any resolution, indoors or outdoors. +2. `terrain.dem_file`: a DEM (height grid) file that `navmap_gis_tool --dem-out` writes. + The plugin waits `terrain.dem_wait_timeout` s for it to exist. + +With neither, the plugin does not start (it says which parameter is missing). + +### A NavMap file (indoors, or any place you have a map of) + +`terrain.initial_navmap` is a path (`~` allowed), or relative to the share directory of +`terrain.package` if you give one (`package: my_robot_maps`, `initial_navmap: maps/site.navmap`). + +- **Grid engine** (`multilevel.enabled: false`, one floor): the NavMap is resampled onto a height + grid of `terrain.initial_navmap_resolution` m (0 = `lengths.cell_size.min`) centred on the map's + own (0, 0, 0): keep the map centred. A node is ground if a NavCel covers it; a gap of one node is + filled, a gap of two or more stays; NaN heights are no data. +- **Hierarchy engine** (`multilevel.enabled: true`, floors over each other): the NavMap's own + triangles are used, so nothing is resampled. Required for a building with several floors. +- `terrain.drop_unreachable_ground: true` keeps only the largest connected piece of ground the robot + can drive on (slope <= `terrain.max_slope_deg`), plus the walls that touch it: roofs, plateaus with + no way up and floors not joined to the rest disappear, so a goal picked there (RViz takes the + topmost surface under the mouse) is impossible instead of unreachable. Use it in cities and + buildings, not for maps of separate pieces; the robot must start on the largest one. +- A `traversed` layer in the file starts that ground as known. A saved map's `obstacle_hits` + counts come back into the obstacle memory. +- `.yaml` next to the file (same name) holds where the map's (0, 0, 0) is on Earth; without + it the map has no geolocation (origin 0, 0, 0): + + ```yaml + georeferenced: true + origin_lat: 40.284796 # WGS84 deg + origin_lon: -3.821407 + reference_altitude: 650.9 # m ASL of z = 0 + min_resolution: 1.0 + ``` + +**How to get a NavMap file:** + +| You have | Do | +|---|---| +| a point cloud (`.pcd`, LiDAR, SLAM, photogrammetry) | `ros2 run navmap_tools pointcloud_to_navmap --input cloud.pcd --output map.navmap --resolution 0.4 --min-clearance 1.0 --recenter` (below) | +| a map the plugin saved | `ros2 service call /maps_manager_node/bigmap/save_map std_srvs/srv/Trigger` writes `map_all_.navmap` and its `.yaml` in `output.save_directory`; use it as `initial_navmap` | +| a mesh of the terrain (STL) | the DEM generator with `elevation_source: mesh` (outdoors, next section) | +| a NavMap from another tool | use it as it is: `navmap_core` format | + +`pointcloud_to_navmap` (package `navmap_tools`, in the NavMap repository) streams clouds of tens of millions of points and writes +`map.navmap` and `map.yaml`. The options that matter: + +- `--resolution`: the finest NavCel; use your `lengths.cell_size.min`. +- `--min-clearance`: free height that separates two floors; it must be **below** the headroom of + the lowest ramp (floors 2.2 m apart: 1.0 to 1.5 keeps them joined, 2.0 cuts the ramp). +- `--max-slope-deg`: surfaces steeper than this stay as gaps (walls). +- `--recenter`: shifts the map so its centre is (0, 0); the shift goes to the `.yaml` as + `offset_x` / `offset_y`. +- `--origin lat,lon,alt`: makes the map georeferenced (the `.yaml` above). +- `--crop x0,y0,x1,y1`, `--zmin`, `--zmax`: keep only a window (a height window drops roofs and + canopies). +- Keep the cloud in local coordinates: a PCD stores float32, 0.5 m of precision at UTM northings. + +The tool tells you if the data holds the resolution you asked. All options: `--help`. + +### Outdoor: getting the map + +Outdoors the plugin reads a **DEM file**: a square grid of heights around a GPS origin (the +`DemGrid` of the plugin, `src/dem_grid.cpp`). The tool that builds it, from open data and +once, is `navmap_gis_tool` of the NavMap repository (package `navmap_tools`) with `--dem-out`. All +the sources are free and need no account or API key: + +| `--dem-source` | Covers | Resolution | Data | +|---|---|---|---| +| `ign` | Spain | 5 m | IGN terrain model (PNOA-LiDAR); the imagery is the PNOA orthophoto (`--imagery-source pnoa`) | +| `copernicus30` (default) | the whole world | 30 m | Copernicus DEM GLO-30 (AWS Open Data) | +| `mesh` | wherever your STL is | your mesh | `--mesh-file`, in the map's local frame | +| `synthetic` | nowhere (a test terrain) | | offline, to try the rest | + +**Step by step** (no launch needed): + +1. Pick the place (`--center lat,lon`, WGS84; it becomes the map's (0, 0)) and the size (`--size`, + the side of the square in metres; the grid has `(size / resolution + 1)^2` nodes), and run the + tool once: + + ```bash + ros2 run navmap_tools navmap_gis_tool --center 40.284796,-3.821407 --size 1000 \ + --resolution 1.0 --dem-source ign --dem-out $HOME/.ros/bigmap/elevation_cache.bin + ``` + + - `--resolution`: m between DEM nodes = the smallest NavCel (`lengths.cell_size.min` should be the + same). The source is sampled at its native spacing (`--dem-data-resolution`, 5 m for `ign`) and + interpolated: a finer grid is smoother, not more detailed. + - `--dem-imagery` (with `--imagery-source pnoa|esri|xyz`): paint the satellite image on the + vertices (RViz colours). They play no part in navigation. + - `--reference-altitude`: the altitude above sea level that is z = 0; by default the height at + the origin. + - The file is written where `--dem-out` says. **It must be + `maps_manager_node.bigmap.terrain.dem_file`** (the example parameter file has it). + - If the file exists and was made with the same origin, size, resolution and source it is reused + and nothing is downloaded (`--force-refresh` builds it again). +2. The tool prints the *reference altitude* (`reference altitude 648.0 m`), the altitude of z = 0. + Set the UKF origin of the localizer to the `--center` and that altitude: + `localizer_node.ukf.latitude_origin`, `longitude_origin` and `altitude_origin` + (`bigmap_bringup`'s launch writes an overrides file with them for you). +3. Run EasyNav with the parameter file. The plugin loads `terrain.dem_file`, builds the NavMap around + the robot and publishes it: + + ```bash + ros2 run easynav_system system_main --ros-args --params-file my_outdoor.yaml + ``` + +Other things the tool takes: + +| Option | What | +|---|---| +| `--dem-source mesh` + `--mesh-file`, `--mesh-world-elevation`, `--mesh-cell` | an STL already in the map's local frame (x east, y north, z up) and the altitude ASL of its z = 0; the area is capped to the mesh's footprint | +| `--terrain-out DIR` + `--terrain-resolution` | also export the DEM as a Gazebo terrain (`terrain.stl` for the collision, `terrain.obj/.mtl/.png` for the visual), in the map frame | +| `--world-template F` + `--world-out W` | with `--terrain-out`: fill `@LATITUDE@`, `@LONGITUDE@`, `@ELEVATION@` and `@TERRAIN_DIR@` of a world template (the simulations of `bigmap_bringup` use it) | + +The same tool makes a Gazebo package of the place (`--gazebo`) or a NavMap of its terrain +(`--navmap`): see its `--help`. + +**Outside Spain.** + +```bash +ros2 run navmap_tools navmap_gis_tool --center 46.0207,7.7491 --size 1000 --resolution 1.0 \ + --dem-source copernicus30 --dem-out $HOME/.ros/bigmap/elevation_cache.bin +``` + +(that place, Zermatt, gives a reference altitude of about 1600 m, as it should.) What to know: + +- The data is 30 m, so the terrain is smoother than the IGN's: ditches, kerbs and walls of a few + metres do not exist in it. For those, make a NavMap of your own (previous section). +- Heights are above sea level; z = 0 is the terrain at the origin unless `--reference-altitude` + says otherwise, as with the IGN. +- **Satellite colours** are optional. `--imagery-source esri` (worldwide, no key) or + `--imagery-source xyz --imagery-url https:////{z}/{x}/{y}.jpg` (any XYZ tile server; + `--zoom` must be one the server has; which server to use, and under what terms, is your choice: + check the provider's terms of use). Without `--dem-imagery` the terrain is built without colours. +- The area world of `easynav_playground_bigmap` (buildings, trees, walls) is Spain only for the Catastro; elsewhere OpenStreetMap. + +**Alternatives outdoors**, if you do not use the download: make a NavMap file of the terrain +(previous section) and set `terrain.initial_navmap` instead of `dem_file`; set the UKF origin by +hand (or from the map's `.yaml`: `origin_lat`, `origin_lon`, `reference_altitude`). + +## Run time + +Services (`//bigmap/...`, all `std_srvs/srv/Trigger`): + +| Service | What | +|---|---| +| `save_map` | writes the whole map (at the current resolutions) to `output.save_directory` as `map_all_.navmap` + `.yaml` | +| `save_traversed` | the same, only the ground the robot has driven over | +| `clear_obstacles` | forgets every obstacle of the memory, the fixed ones too (for a recovery) | + +The write happens in the plugin's next update; the result is in the log. + +Parameters that can be changed while running: `obstacles.memory.enabled` (off: the fixed obstacles +are released at once and detections block as without the memory), `output.view_max_z` (cuts the +map RViz shows, to choose a floor), and the usual `ros2 param set /maps_manager_node bigmap.`. + +Topics: `/bigmap/map` (NavMap, transient local), `.../map_updates`, `.../dynamic_map`, +`.../markers`. + +## Reliability, speed and memory + +- **Holes are obstacles, exactly.** From a NavMap the grid engine keeps, per grid cell, whether the + map has ground there: at full resolution every hole of the map is where it is, however small (a + block of one cell is an obstacle of one cell). `lod.hole_error` (m) makes the border of the holes + count as a height error, so they keep their shape farther away. +- **Obstacles seen from near stay seen** (`lod.observed.enabled`): the hole borders within + `lengths.radii.observed_mark` of the robot stay at full resolution (within + `lengths.radii.observed_full_res` of them) wherever the robot goes, until it is farther than + `traversed_keep` (mode `circles`). A coarse NavCel can no longer close, from far, a hole the + robot saw from near: no plan through it, no back and forth between two meshes. +- **No path, finer map** (`recovery.*`): with a goal and no path for `no_path_cycles` cycles, + `full_res` is doubled (up to `max_full_res_factor` times) and the map re-meshed; back to normal + when the goal is reached or dropped. +- **Re-mesh in a worker** (`lod.async_remesh`, off by default): the cycle that asks for a re-mesh + does not wait for it; the previous map is used until the new one is ready. +- **Hierarchy engine: incremental extraction.** The far cut (what the mesh is with the robot + infinitely far) is computed once; each re-mesh climbs again only the vertices near the robot and + near the driven-over / observed ground. Its cost is that of the region around the robot. +- **Memory.** The NavMap file is read as its arrays (never built as a NavMap only to be read); the + error map is 2 bytes per node (centimetres) and holds no copy of the heights; the hierarchy keeps + compact arrays (colours as bytes, slopes in hundredths of a degree, cells as sorted arrays). The + log prints the size of the mesher at start. + +## Obstacles and filters, in one paragraph + +`obstacles.detector` says who finds the obstacles: `bigmap_dynamic` (the plugin analyses the +sensors: a detection that stays 5 s over a NavCel is an obstacle, `max_hits` of those fix it for +good) or `navmap_filter` (an `ObstaclesFilter` of `easynav_navmap_maps_manager` does, bigmap does +not). Then `removal` takes the obstacle NavCels (and a ring of `lengths.radii.obstacle_clearance`) +out of the map, the **filters** run in the order of the `filters` list (`inflation` writes +`inflated_obstacles`, which the planner reads), and `tint` paints the obstacles. A filter runs if +and only if it is in `filters`. At start the log prints the pipeline in use and refuses (or warns +about) combinations that do nothing, such as the `navmap_filter` detector without an obstacle +filter. Full description: the `bigmap_bringup` README, "Dynamic map and obstacles". + +## Parameters + +All of them, with units, accepted values and defaults, are in the examples above. Invalid values +stop the node at start with a message naming the parameters. Names used by earlier versions +(`lod_settings.*`, `traversed_ground.*`, `obstacle_source`...) are still accepted, with a warning +that says what replaces them. `bigmap.inflation.inflation_radius` must be at least +`system_node.robot_geometry.inscribed_radius`. + +## Related packages + +| Package | What it is | Needed by the plugin? | +|---|---|---| +| `navmap_tools` (NavMap) | `pointcloud_to_navmap` and `navmap_gis_tool`: the tools that make the NavMap and DEM files | no (offline tools) | +| `bigmap_bringup` | the application: launch file (the DEM, EasyNav, RViz), the complete parameter files the examples are copied from, maps; runs on a real robot and on a simulated one | no | +| `easynav_playground_bigmap` | the simulations: Gazebo worlds of a place with the Summit | no | + +## Tests + +`colcon test --packages-select easynav_bigmap_maps_manager`: unit tests of the meshers, the obstacle +memory, the pipeline check and the DEM from a NavMap; a test of the plugin itself (parameters, +`initialize()`, `update()` with a pose and sensor points); and a regression test of the hierarchy +engine on the real parking-garage map. Tests that need a real map of your own are skipped unless +`BIGMAP_TEST_NAVMAP=` is set. Build with `-DCMAKE_BUILD_TYPE=Release` (the meshers are +slow in Debug). + +## Author and license + +Copyright 2026 José Miguel Guerrero (), author and maintainer. +Licensed under the Apache License, Version 2.0 (see `LICENSE` in the repository root). diff --git a/maps_managers/easynav_bigmap_maps_manager/config/example_indoor.yaml b/maps_managers/easynav_bigmap_maps_manager/config/example_indoor.yaml new file mode 100644 index 00000000..6acce505 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/config/example_indoor.yaml @@ -0,0 +1,424 @@ +# ============================================================================================= +# bigmap - EXAMPLE, INDOOR (a NavMap file; several floors, ramps), REAL ROBOT +# Copy of bigmap_bringup/config/params_indoor.yaml below this header (a test keeps them equal). +# To use it, edit in maps_manager_node.ros__parameters.bigmap: +# terrain.initial_navmap the NavMap file (section 2.1; README, "Where the map comes from") +# terrain.package "" for a path, or a package name for a path inside its share/ +# and in the other nodes: sensors_node (the lidar topic and frame), system_node.robot_geometry +# (the robot's size). Nothing is simulated: use_sim_time false in every node; the robot is +# localized by something else that gives the TF map -> odom (localizer_node.external). +# Run EasyNav with it: ros2 run easynav_system system_main --ros-args --params-file +# +# WHO OWNS WHAT. Each node block belongs to one plugin and only that plugin reads it, except the +# SHARED VALUES below. [owner] tags in the comments say it again where it matters. +# dem_generator navmap_gis_tool (navmap_tools), run by the launch [OUTDOOR ONLY] +# maps_manager_node.bonxai easynav_bonxai_maps_manager: the map AMCL localizes on [SIMULATION] +# maps_manager_node.bigmap easynav_bigmap_maps_manager (this repo): the LOD NavMap +# .filters .inflation .obstacle_filter easynav_navmap_maps_manager (plugins bigmap borrows) +# controller_node. easynav_regulated_pp_controller +# localizer_node. easynav_navmap_localizer (amcl) | easynav_fusion_localizer (ukf) +# | external +# planner_node.astar easynav_navmap_planner +# sensors_node easynav_sensors +# system_node easynav_system +# +# SHARED VALUES (who defines it -> who reads it): +# system_node.robot_geometry.inscribed_radius -> every plugin (MethodBase), in particular bigmap's +# inflation filter: inflation_radius >= inscribed_radius +# maps_manager_node.bonxai (cloud, resolution) -> localizer_node.amcl (the map it localizes on) +# system_node.position_tolerance / angle_tolerance -> controller xy/yaw_goal_tolerance (keep equal) +# sensors_node. (topic, frame) -> bigmap's dynamic map and obstacle memory, and the +# obstacle filter, through the NavState +# use_sim_time: the same in EVERY node (true in simulation, false on the real robot) +# +# HOW OPTIONS ARE WRITTEN. `key: value # [owner] what it is, unit, accepted values, what happens +# on/off, what it needs`. On/off switches are always `enabled: true|false` inside their block +# (traversed, multilevel, obstacles.removal/memory/tint, dynamic_map, ground_fit, output.profile); +# a NavMapFilter has no switch of its own: it runs if it is in `filters` (2.7). Defaults are the +# plugin's; this file gives the values of the scenario. +# +# bigmap SECTIONS (2.x of maps_manager_node.bigmap): +# 2.1 terrain where the map comes from, slope limit, narrow passages, hierarchy engine +# 2.2 lengths EVERY radius, cell size and distance (see the table below) +# 2.3 lod height error of the simplification +# 2.4 traversed resolution kept over the ground the robot has driven over +# 2.5 obstacles detector -> memory -> removal -> (inflation, 2.7) -> tint, in that order +# 2.6 dynamic_map what the sensors add (+ ground_fit) +# 2.7 filters the NavMap filters borrowed from easynav_navmap_maps_manager +# 2.8 output markers, RViz cut, saving, profiling +# +# LENGTHS (metres). Circles around the robot, from the smallest to the largest, with what ties them: +# +# bigmap.lengths.radii.traversed_mark ground this close to the robot counts as driven over +# bigmap.lengths.radii.traversed_full_res full resolution strip around driven-over ground +# bigmap.lengths.radii.obstacle_clearance NavCels this close to an obstacle are removed too +# bigmap.lengths.radii.sensor_range <= sensor points analysed (0 = full_res) +# bigmap.lengths.radii.full_res BLUE circle: every NavCel at full resolution +# bigmap.lengths.radii.low_res_start >= from here on NavCels may be as big as cell_size.max +# bigmap.lengths.radii.traversed_keep >= ORANGE circle (traversed.mode: circles only) +# +# in order: traversed_mark < full_res <= low_res_start, sensor_range <= full_res <= traversed_keep +# (obstacle_clearance and traversed_full_res are independent small lengths; the inflation radius is +# bigmap.inflation.inflation_radius, and the robot's own size system_node.robot_geometry) +# bigmap.lengths.cell_size.{min,max} smallest and largest NavCel (min <= max) +# bigmap.lengths.traversed_fade distance over which resolution fades beyond the strip +# bigmap.lengths.remesh_step how far the robot moves before the map is re-meshed +# +# Names used before this reorganisation are still accepted (a warning lists them); e.g. +# lod_settings.high_res_radius = lengths.radii.full_res, dynamic_map.max_range = +# lengths.radii.sensor_range, obstacles.source = obstacles.detector (dynamic = bigmap_dynamic, +# filter = navmap_filter). +# ============================================================================================= + +# --------------------------------------------------------------------------------------------- +# 1. DEM GENERATOR [navmap_gis_tool] [OUTDOOR ONLY]: nothing here, the map is a file (bigmap.terrain). +# --------------------------------------------------------------------------------------------- + +# --------------------------------------------------------------------------------------------- +# 2. MAPS MANAGER = easynav_bigmap_maps_manager/BigMapMapsManager (dynamic-LOD NavMap) +# --------------------------------------------------------------------------------------------- +maps_manager_node: + ros__parameters: + use_sim_time: false + map_types: [bigmap] + bigmap: + plugin: easynav_bigmap_maps_manager/BigMapMapsManager # [bigmap] the plugin of this repo + freq: 10.0 # [easynav] Hz of update(): remesh check, obstacles, filters + + # ====================================================================================== + # 2.1 TERRAIN [bigmap]: where the map comes from, what is too steep, which engine + # ====================================================================================== + terrain: + package: "" # "": initial_navmap is a path ("~" allowed); a package name: it is + # relative to that package's share directory (like bonxai's) + initial_navmap: "" # NavMap file the map is made of (any resolution; a `traversed` layer + # starts that ground as known; .yaml next to it = origin + # (geolocation, offsets). ""+ dem_file: the DEM instead. Required + # when multilevel.enabled + initial_navmap_resolution: 0.0 # m, grid engine: resolution of the grid made from the file + # (0 = lengths.cell_size.min) + max_slope_deg: 30.0 # deg, >0: NavCels steeper than this are LETHAL_OBSTACLE for the planner + # (the `occupancy` layer, red in RViz) and count as fixed obstacles + # in `obstacle_hits`. Also decides what a narrow passage is + keep_narrow_width: 3.0 # m, >=0, 0 = off. BOTH engines: ramps, corridors, doors narrower than + # this stay at the map's full resolution however far the robot is + # (else the planner cannot find the way through from far away). + # Width = free ground between ground steeper than max_slope_deg + # (or no data). Costs triangles where narrow places are many. + # 3.0 here: the ramps and corridors of this map stay fine from far away. + drop_unreachable_ground: true # BOTH engines: ground the robot cannot get to is dropped: only the + # largest connected piece of drivable ground (slope <= max_slope_deg) + # is kept, with the faces of walls and slopes that touch it. The roofs + # of buildings, a plateau with no way up, a floor not joined to the + # rest: a goal picked there (the tool takes the topmost surface under + # the mouse) could never be reached. Not for a map made of several + # separate pieces the robot must reach (it keeps only the largest) + + # Engine of the hierarchy mode [bigmap]: the NavMap's own triangles are merged by clustering + # (multilevel.enabled: on: floors over each other and ramps) + multilevel: + enabled: true # true: hierarchy engine (needs terrain.initial_navmap). false: grid engine + cluster_height: 1.2 # m: a cluster never spans more height (floors are never merged) + vertical_tolerance: 1.5 # m: ground driven over is the one this close in z + vertical_weight: 4.0 # height counts this many times in the distance to the robot + + # ====================================================================================== + # 2.2 LENGTHS [bigmap]: EVERY radius and size in one place (table at the top of the file) + # ====================================================================================== + lengths: + radii: + traversed_mark: 1.0 # m: ground this close to the robot counts as driven over (`traversed`) + traversed_full_res: 1.0 # m: full-resolution strip around driven-over ground (0 = full_res) + observed_mark: 5.0 # m, = full_res: the borders within it are seen at full resolution (kept so) + observed_full_res: 1.0 # m: full-resolution strip around those borders + obstacle_clearance: 0.3 # m: NavCels this close to an obstacle are removed/blocked too + # (small: walls are the map's borders already; a wide ring would + # close ramps and doors). Used by obstacles.removal + sensor_range: 0.0 # m, <= full_res: only sensor points this close to the robot are + # analysed (dynamic map, obstacle memory). 0 = full_res + full_res: 5.0 # m: BLUE circle: every NavCel at full resolution (cell_size.min) + low_res_start: 20.0 # m, >= full_res: from here NavCels as big as cell_size.max; between + # full_res and this the size grows geometrically + traversed_keep: 15.0 # m, >= full_res: ORANGE circle (traversed.mode: circles only) + cell_size: + min: 0.4 # m: smallest NavCel: the map's own resolution (a narrow ramp needs NavCel centres 0.5 m from its sides) + max: 16.0 # m, >= min: largest NavCel leg + traversed_fade: 5.0 # m: beyond the strip the NavCels grow from min to max over this distance + # (a NavCel cannot exceed ~half its distance to the full-resolution + # ground: the mesh must stay crack-free) + remesh_step: 3.0 # m: the robot must move this far to trigger a re-mesh + + # ====================================================================================== + # 2.3 LOD [bigmap]: height error the simplification may make (grid engine) + # ====================================================================================== + lod: + base_error: 0.02 # m: inside the full_res circle + error_growth: 0.05 # m of extra tolerance per metre beyond full_res + # the holes of the map (its obstacles): their border counts as this height error (m), so + # they keep their shape as far as the tolerance stays below it (0: off) + hole_error: 0.0 + # obstacle borders seen from near (lengths.radii.observed_mark) stay at full resolution + # wherever the robot goes: no plan through an obstacle already seen, no back and forth + # between two meshes + observed: + enabled: true + # re-mesh in a worker thread: the cycle does not wait for it, the previous map is used + # until the new one is ready + async_remesh: false + # with a goal and no path for no_path_cycles cycles the full_res circle is doubled (up to + # max_full_res_factor times) and the map re-meshed, until the goal is reached or dropped + recovery: + enabled: true + no_path_cycles: 3 + max_full_res_factor: 4.0 + + # ====================================================================================== + # 2.4 TRAVERSED GROUND [bigmap]: known navigable, high confidence (layer `traversed`) + # small circle = lengths.radii.full_res (blue), large = lengths.radii.traversed_keep (orange) + # ====================================================================================== + traversed: + enabled: true # false: the robot's path is not remembered (no layer, no fine strip) + mode: circles # circles: inside the small circle all at full resolution; between the two only + # the ground driven over; beyond the large one low resolution, and + # what was driven over there is FORGOTTEN (layer and saved maps too) + # persistent: inside the small circle all fine; outside it only the + # ground driven over, wherever it is, for good + + # ====================================================================================== + # 2.5 OBSTACLES [bigmap]: how what the sensors detect gets into the `obstacles` layer. + # In the order it is applied (the dependencies are checked at start and logged): + # detector -> memory -> removal -> (filters 2.7: inflation) -> tint + # A* reads `inflated_obstacles` if the inflation filter is in `filters`, else `obstacles`. + # Terrain steeper than terrain.max_slope_deg is always an obstacle, whatever is set here. + # ====================================================================================== + obstacles: + # WHO detects obstacles with the sensors (the only way that decides it): + # bigmap_dynamic bigmap's own dynamic map (2.6): heights relative to the robot, independent + # of the localization's z. Needs removal.enabled to reach the planner. + # navmap_filter the ObstacleFilter plugin of easynav_navmap_maps_manager: heights through + # the map -> robot TF (needs a good z). Needs it in `filters` (2.7); + # memory is off with it. + # (before: `source: dynamic | filter`, still accepted) + detector: bigmap_dynamic + removal: # [bigmap_dynamic] NavCels with a detection, and those within + enabled: true # lengths.radii.obstacle_clearance of one, are REMOVED from the + # published NavMap (holes in RViz) and set LETHAL in `obstacles`. + # false: detections only reach the dynamic map, NOT the planner + memory: # [bigmap_dynamic] layer `obstacle_hits` (uint8 per NavCel) + enabled: true # true: an obstacle is a detection that stays min_time over a NavCel, + # and the layer counts how often. The sensors are analysed even + # if dynamic_map.enabled is false. false: a detection blocks after + # dynamic_map.confirm_time and goes after forget_time (and nothing + # is analysed if dynamic_map.enabled is false). Runtime: + # ros2 param set /maps_manager_node bigmap.obstacles.memory.enabled false + # (a recovery that takes care of obstacles itself can do that): + # the fixed obstacles are released in that cycle; the counts are + # kept and come back when it is switched on again + min_time: 5.0 # s a detection must stay over the same NavCel (gaps over 1 s restart + # the count) to be an obstacle = one hit; one more per min_time s + # it goes on or comes back + max_hits: 3 # 1..255 hits that FIX the obstacle: blocked for good, seen or not + # (about max_hits x min_time s of detection). Below it the NavCel + # blocks only while detected. Steep terrain shows max_hits in the + # layer. 1: the first one is permanent. A fixed obstacle blocks + # only NavCels up to 4 x cell_size.min: far away it waits until the + # mesh there is fine + tint: # [bigmap] display only + enabled: true # paint the `obstacles` NavCels over the imagery in RViz + color: [255, 40, 40] # RGB + strength: 0.65 # 0 = imagery only, 1 = flat colour + + # ====================================================================================== + # 2.6 DYNAMIC MAP [bigmap]: what the sensors see above a NavCel raises it to that height. + # Heights are relative to the robot and referred to the terrain under it (not through the + # map -> robot TF, whose z drifts). Published at /bigmap/dynamic_map and NavState + # "map.dynamic"; the static map is untouched. Analysed if enabled OR obstacles.memory.enabled + # (only inside lengths.radii.sensor_range; the cost grows with that area). + # ====================================================================================== + dynamic_map: + enabled: true # true: analyse and publish the dynamic map (its detections block the planner too, + # after confirm_time, unless obstacles.memory.enabled). false: neither + # (the memory may still analyse the sensors) + # SIZE OF AN OBSTACLE: a point counts if it is at least min_height above the plane of the NavCel + # it falls in, along its normal, with the robot's roll/pitch/yaw taken into account: a ramp the + # robot drives up is the NavCel's own plane and never an obstacle. + min_height: 0.3 # m: lower = also small objects; higher = bumps and noise ignored + min_vertical_extent: 0.2 # m: ...and the points of one cell span at least this in height (flat + # ground the map puts a bit low is not an obstacle). Keep it above + # 0.25 m x tan(steepest slope) + confirm_time: 0.3 # s: ...and it is seen in the same cell this long (fleeting errors are + # not obstacles). Not used as the obstacle rule when memory is on + max_height: 1.5 # m above the robot's ground: higher points (ceilings, the floor above) ignored + forget_time: 5.0 # s a detection stays without being seen again + cell: 0.25 # m: detections are merged on this grid + voxel: 0.1 # m: sensor clouds are thinned to one point per voxel first (bigger = cheaper) + # GROUND MODEL FROM THE SENSOR: in a window in front of the robot a plane is fitted (RANSAC) to + # the lowest point of each cell: the ramp being climbed. Whatever lies on it is ground. + ground_fit: + enabled: true # false: only the map decides what is ground + window_length: 4.0 # m ahead of the robot + window_width: 3.0 # m across, centred on the robot + z_min: -1.0 # m (robot frame): lower points are not used for the plane + z_max: 1.2 # m: higher points are not used for the plane + tolerance: 0.08 # m: a lowest point this close to the plane is ground + max_tilt_deg: 40.0 # deg: a plane steeper than this is a wall, not ground + min_cells: 12 # fewer cells than this: only the map decides + + # ====================================================================================== + # 2.7 NAVMAP FILTERS [easynav_navmap_maps_manager]: plugins bigmap borrows, not its own. + # `filters` is the ONLY on/off switch of each of them: a filter runs if and only if it is + # listed here (and has `.plugin`). Instance names are free, except that `obstacles` + # is bigmap's own section (2.5): name an obstacle filter `obstacle_filter`. + # What a filter is, is told by the layer it writes: + # inflation -> inflated_obstacles independent of obstacles.detector + # obstacle_filter -> obstacles only with obstacles.detector: navmap_filter + # [inflation] inflation on, sensors by bigmap (default) + # [obstacle_filter, inflation] + detector: navmap_filter (obstacles.memory is then off) + # [] no inflation: A* reads `obstacles` and keeps no distance + # Listing obstacle_filter with detector bigmap_dynamic, or the reverse, is reported at start. + # ====================================================================================== + filters: [inflation] + inflation: + plugin: easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter + # NavCels within system_node.robot_geometry.inscribed_radius of an obstacle are blocked + # (shared: system_node defines it, this filter reads it); up to inflation_radius the cost + # decays and keeps the paths away from obstacles (and from the map border) + inflation_radius: 2.0 # m, >= system_node.robot_geometry.inscribed_radius + cost_scaling_factor: 1.5 # >0: how fast the cost decays with the distance + obstacle_filter: # only loaded if listed in `filters` + plugin: easynav_navmap_maps_manager/NavMapMapsManager/ObstaclesFilter + max_range: 10.0 # m: points farther than this from the robot are ignored + min_height: 0.1 # m above the NavMap surface under them: lower points are ground + downsample_resolution: 0.3 # m: voxel the points are reduced to first (<= 0: off) + + # ====================================================================================== + # 2.8 OUTPUT [bigmap] + # ====================================================================================== + output: + publish_markers: true # the two circles etc. as RViz markers + # RViz shows the map only up to this height (a click lands on a lower floor); runtime: + # ros2 param set /maps_manager_node bigmap.output.view_max_z 3.0 + # Only the displayed map: the planner's and the saved ones are complete. 1000 = everything. + view_max_z: 1000.0 # m + # Save the map as it is now (with the resolutions it has now) as a NavMap file: + # ros2 service call /maps_manager_node/bigmap/save_map std_srvs/srv/Trigger (all) + # ros2 service call /maps_manager_node/bigmap/save_traversed std_srvs/srv/Trigger (only the + # NavCels the robot has driven over). The reply says the file written. + save_directory: "~/.ros/bigmap/saved" + # Timing of the sections of the maps manager: every `period` s a table in the log with the + # mean / worst ms per call and the ms spent per second of each (the first rows cost most). + profile: + enabled: false # true: log that table + period: 5.0 # s + +# --------------------------------------------------------------------------------------------- +# 3. CONTROLLER [easynav_regulated_pp_controller]: Regulated Pure Pursuit (same in all scenarios). +# Nothing of bigmap's own is read here. +# --------------------------------------------------------------------------------------------- +controller_node: + ros__parameters: + use_sim_time: false + robot_limits: + max_linear_vel: 1.0 + min_linear_vel: -0.2 + max_angular_vel: 1.0 + max_linear_acc: 0.5 + max_linear_decel: 1.0 + max_angular_acc: 1.0 + max_angular_decel: 2.0 + controller_types: [regulated] + regulated: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Area where obstacles regulate the velocity. + safety_margin: 0.1 + use_dynamic_window: false + allow_reversing: false + # Lookahead, scaled for the Summit XL + lookahead_dist: 1.0 + min_lookahead_dist: 0.5 + max_lookahead_dist: 1.5 + lookahead_time: 1.5 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 0.8 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 1.5 + regulated_linear_scaling_min_speed: 0.2 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.5 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 1.0 + z_min_filter: 0.3 # ignore the ground seen by the 3D lidar and the depth camera + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 1.0 + # Goal tolerances (system_node: 0.3 m / 0.15 rad) + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.15 + +# --------------------------------------------------------------------------------------------- +# 4. LOCALIZER [REAL ROBOT] [external]: the robot is localized by something else, which must give the TF +# map -> odom (odom -> base_footprint -> base_link comes from the robot). The simulations +# (params_parking_garage.yaml, params_large_warehouse.yaml) use AMCL instead. +# --------------------------------------------------------------------------------------------- +localizer_node: + ros__parameters: + use_sim_time: false + localizer_types: [external] + external: + rt_freq: 50.0 + freq: 5.0 + plugin: easynav_localizer/DummyLocalizer + +# --------------------------------------------------------------------------------------------- +# 5. PLANNER [easynav_navmap_planner]: A* over the NavMap (same in all scenarios). +# Cost layer: `inflated_obstacles` if bigmap.filters has the inflation, else `obstacles`. +# --------------------------------------------------------------------------------------------- +planner_node: + ros__parameters: + use_sim_time: false + planner_types: [astar] + astar: + freq: 1.0 + plugin: easynav_navmap_planner/AStarPlanner + # the map changes with the robot's position (level of detail): keep the path being followed + # while its first replan_hysteresis_horizon m are traversable, unless a new one is 20 % + # shorter (0: always the new one; horizon 0: the whole path is checked) + replan_hysteresis: 0.2 + replan_hysteresis_horizon: 5.0 # m, ~ bigmap full_res: beyond it the map is coarse and changes + +# --------------------------------------------------------------------------------------------- +# 6. SENSORS [easynav_sensors] [INDOOR]: the robot's 3D lidar only +# --------------------------------------------------------------------------------------------- +sensors_node: + ros__parameters: + use_sim_time: false + forget_time: 0.5 + sensors: [laser] + perception_default_frame: odom + laser: + topic: front_laser/points # the robot's 3D lidar (sensor_msgs/PointCloud2) + type: sensor_msgs/msg/PointCloud2 + +# --------------------------------------------------------------------------------------------- +# 7. SYSTEM [easynav_system] +# --------------------------------------------------------------------------------------------- +system_node: + ros__parameters: + use_sim_time: false + use_real_time: true + rt_freq: 50.0 + position_tolerance: 0.3 + angle_tolerance: 0.15 + robot_geometry: # SHARED: read by every plugin; bigmap.inflation.inflation_radius >= inscribed_radius + radius: 0.5 # m: Summit XL (circle around its 0.72 x 0.61 m footprint); 0.7 is + # too much for a 2 m ramp between curbs + inscribed_radius: 0.5 # m: NavCels closer than this to an obstacle are blocked + height: 1.0 # m diff --git a/maps_managers/easynav_bigmap_maps_manager/config/example_outdoor.yaml b/maps_managers/easynav_bigmap_maps_manager/config/example_outdoor.yaml new file mode 100644 index 00000000..574040bc --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/config/example_outdoor.yaml @@ -0,0 +1,529 @@ +# ============================================================================================= +# bigmap - EXAMPLE, OUTDOOR (GPS terrain, DEM downloaded once: Spanish IGN or worldwide) +# Copy of bigmap_bringup/config/params_outdoor.yaml below this header (a test keeps them equal). +# It is the file of the Gazebo simulation: use_sim_time true and the simulated robot's topics; +# on a real robot set use_sim_time false in every node and your sensors' topics. +# To use it, edit the place (section 1: gps_origin, WGS84, and download_radius; the launch reads +# them, the command below takes them as options), then (README, "Outdoor: getting the map"): +# 1. ros2 run navmap_tools navmap_gis_tool --center --size <2 * download_radius> \ +# --resolution --dem-source ign|copernicus30 --dem-out +# (downloads the terrain once and writes the DEM file = cache_file = terrain.dem_file; it +# prints the reference altitude: the altitude of z = 0) +# 2. ros2 run easynav_system system_main --ros-args --params-file +# with localizer_node.ukf.latitude_origin / longitude_origin / altitude_origin set to the +# centre and that reference altitude (bigmap_bringup's launch does it for you) +# +# WHO OWNS WHAT. Each node block belongs to one plugin and only that plugin reads it, except the +# SHARED VALUES below. [owner] tags in the comments say it again where it matters. +# dem_generator navmap_gis_tool (navmap_tools), run by the launch [OUTDOOR ONLY] +# maps_manager_node.bonxai easynav_bonxai_maps_manager: the map AMCL localizes on [SIMULATION] +# maps_manager_node.bigmap easynav_bigmap_maps_manager (this repo): the LOD NavMap +# .filters .inflation .obstacle_filter easynav_navmap_maps_manager (plugins bigmap borrows) +# controller_node. easynav_regulated_pp_controller +# localizer_node. easynav_navmap_localizer (amcl) | easynav_fusion_localizer (ukf) +# | external +# planner_node.astar easynav_navmap_planner +# sensors_node easynav_sensors +# system_node easynav_system +# +# SHARED VALUES (who defines it -> who reads it): +# system_node.robot_geometry.inscribed_radius -> every plugin (MethodBase), in particular bigmap's +# inflation filter: inflation_radius >= inscribed_radius +# maps_manager_node.bonxai (cloud, resolution) -> localizer_node.amcl (the map it localizes on) +# system_node.position_tolerance / angle_tolerance -> controller xy/yaw_goal_tolerance (keep equal) +# sensors_node. (topic, frame) -> bigmap's dynamic map and obstacle memory, and the +# obstacle filter, through the NavState +# use_sim_time: the same in EVERY node (true in simulation, false on the real robot) +# +# HOW OPTIONS ARE WRITTEN. `key: value # [owner] what it is, unit, accepted values, what happens +# on/off, what it needs`. On/off switches are always `enabled: true|false` inside their block +# (traversed, multilevel, obstacles.removal/memory/tint, dynamic_map, ground_fit, output.profile); +# a NavMapFilter has no switch of its own: it runs if it is in `filters` (2.7). Defaults are the +# plugin's; this file gives the values of the scenario. +# +# bigmap SECTIONS (2.x of maps_manager_node.bigmap): +# 2.1 terrain where the map comes from, slope limit, narrow passages, hierarchy engine +# 2.2 lengths EVERY radius, cell size and distance (see the table below) +# 2.3 lod height error of the simplification +# 2.4 traversed resolution kept over the ground the robot has driven over +# 2.5 obstacles detector -> memory -> removal -> (inflation, 2.7) -> tint, in that order +# 2.6 dynamic_map what the sensors add (+ ground_fit) +# 2.7 filters the NavMap filters borrowed from easynav_navmap_maps_manager +# 2.8 output markers, RViz cut, saving, profiling +# +# LENGTHS (metres). Circles around the robot, from the smallest to the largest, with what ties them: +# +# bigmap.lengths.radii.traversed_mark ground this close to the robot counts as driven over +# bigmap.lengths.radii.traversed_full_res full resolution strip around driven-over ground +# bigmap.lengths.radii.obstacle_clearance NavCels this close to an obstacle are removed too +# bigmap.lengths.radii.sensor_range <= sensor points analysed (0 = full_res) +# bigmap.lengths.radii.full_res BLUE circle: every NavCel at full resolution +# bigmap.lengths.radii.low_res_start >= from here on NavCels may be as big as cell_size.max +# bigmap.lengths.radii.traversed_keep >= ORANGE circle (traversed.mode: circles only) +# +# in order: traversed_mark < full_res <= low_res_start, sensor_range <= full_res <= traversed_keep +# (obstacle_clearance and traversed_full_res are independent small lengths; the inflation radius is +# bigmap.inflation.inflation_radius, and the robot's own size system_node.robot_geometry) +# bigmap.lengths.cell_size.{min,max} smallest and largest NavCel (min <= max) +# bigmap.lengths.traversed_fade distance over which resolution fades beyond the strip +# bigmap.lengths.remesh_step how far the robot moves before the map is re-meshed +# +# Names used before this reorganisation are still accepted (a warning lists them); e.g. +# lod_settings.high_res_radius = lengths.radii.full_res, dynamic_map.max_range = +# lengths.radii.sensor_range, obstacles.source = obstacles.detector (dynamic = bigmap_dynamic, +# filter = navmap_filter). +# ============================================================================================= + +# --------------------------------------------------------------------------------------------- +# 1. DEM GENERATOR [OUTDOOR ONLY]: downloads the terrain (and imagery) once, writes the base mesh +# and the DEM that maps_manager_node.bigmap.terrain.dem_file reads, then exits. +# Heights and imagery come from the IGN (Spain): free, no API key. +# --------------------------------------------------------------------------------------------- +dem_generator: + ros__parameters: + # --- where on Earth + gps_origin: + # Same origin as of easynav_playground_summit_worlds/worlds/urjc_excavation.world + latitude: 40.284796 # WGS84 deg (map-frame origin = Gazebo world origin) + longitude: -3.821407 # WGS84 deg + altitude: 0.0 # m ASL mapped to z = 0; 0.0 -> DEM elevation at the origin (keeps the + # mesh under the robot, which spawns on the ground at the origin) + download_radius: 500.0 # m, half-side of the square area around the origin + # --- heights + # Where the heights come from: + # ign - Spanish IGN terrain model (PNOA-LiDAR), 5 m native, free, no key, Spain only + # copernicus30 - worldwide, free, no key: Copernicus DEM GLO-30, 30 m + # mesh - a survey mesh (STL) in the map's frame: mesh_file, mesh_world_elevation + # synthetic - fake terrain for tests (or use_synthetic_terrain: true) + # The source is queried every elevation_resolution m (0 = its native spacing: ign 5, + # copernicus30 30) + # and the result is bicubically interpolated to grid_resolution: finer = smoother terrain and + # small NavCels near the robot, NOT more real detail. + elevation_source: ign + elevation_resolution: 0.0 # m between queries (0 = native) + grid_resolution: 1.0 # m between nodes of the DEM / smallest NavCel (radius 500 -> 1001^2) + use_synthetic_terrain: false # true -> offline fake terrain (tests/demos, no network) + # --- satellite image draped on the mesh + imagery: + enabled: true + # ign - Spanish PNOA orthophoto: free, no key, ~0.25 m, Spain only + source: ign # ign (Spain) | xyz (any tile server: set url_template) | auto (= ign) + url_template: "" # xyz only: https:////{z}/{y}/{x}. No default: the server + # and its terms of use are your choice. Outside Spain and without + # it, set enabled: false (no colours) + zoom: 18 # ~0.45 m per point at 40N; 16 tiles for the default 1 km area + scale: 2 # 1 or 2 (pixel density per tile) + max_tiles: 100 # safety cap on the number of requests + # --- Gazebo terrain: the launch also sets gazebo.export / world_template / world_file: it + # exports this same DEM as the Gazebo terrain (collision STL + textured OBJ), so the robot + # drives on the NavMap's relief + gazebo: + terrain_dir: "~/.ros/bigmap/gazebo" + terrain_resolution: 4.0 # m: Gazebo collision mesh (coarser than the grid: millions of triangles) + # --- files and output + base_navmap: + enabled: false # full-resolution NavMap (millions of triangles at fine grids) + cache_file: "~/.ros/bigmap/elevation_cache.bin" # avoid downloading again on restart + output_navmap_file: "~/.ros/bigmap/base.navmap" # base mesh consumed by easynav_bigmap_maps_manager + map_frame: "map" + publish_topic: "navmap_base" # transient_local, for debugging in RViz + exit_when_done: false # the launch sets it to true: EasyNav starts once the DEM is on disk + + +# --------------------------------------------------------------------------------------------- +# 2. MAPS MANAGER = easynav_bigmap_maps_manager/BigMapMapsManager (dynamic-LOD NavMap) +# --------------------------------------------------------------------------------------------- +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [bigmap] + bigmap: + plugin: easynav_bigmap_maps_manager/BigMapMapsManager # [bigmap] the plugin of this repo + freq: 10.0 # [easynav] Hz of update(): remesh check, obstacles, filters + + # ====================================================================================== + # 2.1 TERRAIN [bigmap]: where the map comes from, what is too steep, which engine + # ====================================================================================== + terrain: + dem_file: "~/.ros/bigmap/elevation_cache.bin" # [OUTDOOR] the DEM the generator writes + dem_wait_timeout: 60.0 # s [OUTDOOR] how long to wait for that file at start + package: "" # "": initial_navmap is a path ("~" allowed); a package name: it is + # relative to that package's share directory (like bonxai's) + initial_navmap: "" # NavMap file the map is made of (any resolution; a `traversed` layer + # starts that ground as known; .yaml next to it = origin + # (geolocation, offsets). ""+ dem_file: the DEM instead. Required + # when multilevel.enabled + initial_navmap_resolution: 0.0 # m, grid engine: resolution of the grid made from the file + # (0 = lengths.cell_size.min) + max_slope_deg: 25.0 # deg, >0: NavCels steeper than this are LETHAL_OBSTACLE for the planner + # (the `occupancy` layer, red in RViz) and count as fixed obstacles + # in `obstacle_hits`. Also decides what a narrow passage is + keep_narrow_width: 0.0 # m, >=0, 0 = off. BOTH engines: ramps, corridors, doors narrower than + # this stay at the map's full resolution however far the robot is + # (else the planner cannot find the way through from far away). + # Width = free ground between ground steeper than max_slope_deg + # (or no data). Costs triangles where narrow places are many. + # 0 here: outdoors the gaps between trees, walls and buildings + # are everywhere, so keeping them fine would keep most of the map fine. + drop_unreachable_ground: true # BOTH engines: ground the robot cannot get to is dropped: only the + # largest connected piece of drivable ground (slope <= max_slope_deg) + # is kept, with the faces of walls and slopes that touch it. The roofs + # of buildings, a plateau with no way up, a floor not joined to the + # rest: a goal picked there (the tool takes the topmost surface under + # the mouse) could never be reached. Not for a map made of several + # separate pieces the robot must reach (it keeps only the largest) + + # Engine of the hierarchy mode [bigmap]: the NavMap's own triangles are merged by clustering + # (multilevel.enabled: off: single-floor terrain (the grid engine)) + multilevel: + enabled: false # true: hierarchy engine (needs terrain.initial_navmap). false: grid engine + cluster_height: 1.2 # m: a cluster never spans more height (floors are never merged) + vertical_tolerance: 1.5 # m: ground driven over is the one this close in z + vertical_weight: 4.0 # height counts this many times in the distance to the robot + + # ====================================================================================== + # 2.2 LENGTHS [bigmap]: EVERY radius and size in one place (table at the top of the file) + # ====================================================================================== + lengths: + radii: + traversed_mark: 1.0 # m: ground this close to the robot counts as driven over (`traversed`) + traversed_full_res: 1.0 # m: full-resolution strip around driven-over ground (0 = full_res) + observed_mark: 20.0 # m, = full_res: the borders within it are seen at full resolution (kept so) + observed_full_res: 1.0 # m: full-resolution strip around those borders + obstacle_clearance: 1.0 # m: NavCels this close to an obstacle are removed/blocked too + # (small: walls are the map's borders already; a wide ring would + # close ramps and doors). Used by obstacles.removal + sensor_range: 0.0 # m, <= full_res: only sensor points this close to the robot are + # analysed (dynamic map, obstacle memory). 0 = full_res + full_res: 20.0 # m: BLUE circle: every NavCel at full resolution (cell_size.min) + low_res_start: 80.0 # m, >= full_res: from here NavCels as big as cell_size.max; between + # full_res and this the size grows geometrically + traversed_keep: 50.0 # m, >= full_res: ORANGE circle (traversed.mode: circles only) + cell_size: + min: 1.0 # m: smallest NavCel: the DEM's grid_resolution (dem_generator) + max: 32.0 # m, >= min: largest NavCel leg + traversed_fade: 5.0 # m: beyond the strip the NavCels grow from min to max over this distance + # (a NavCel cannot exceed ~half its distance to the full-resolution + # ground: the mesh must stay crack-free) + remesh_step: 2.0 # m: the robot must move this far to trigger a re-mesh + + # ====================================================================================== + # 2.3 LOD [bigmap]: height error the simplification may make (grid engine) + # ====================================================================================== + lod: + base_error: 0.02 # m: inside the full_res circle + error_growth: 0.05 # m of extra tolerance per metre beyond full_res + # the holes of the map (its obstacles): their border counts as this height error (m), so + # they keep their shape as far as the tolerance stays below it (0: off) + hole_error: 0.0 + # obstacle borders seen from near (lengths.radii.observed_mark) stay at full resolution + # wherever the robot goes: no plan through an obstacle already seen, no back and forth + # between two meshes + observed: + enabled: true + # re-mesh in a worker thread: the cycle does not wait for it, the previous map is used + # until the new one is ready + async_remesh: false + # with a goal and no path for no_path_cycles cycles the full_res circle is doubled (up to + # max_full_res_factor times) and the map re-meshed, until the goal is reached or dropped + recovery: + enabled: true + no_path_cycles: 3 + max_full_res_factor: 4.0 + + # ====================================================================================== + # 2.4 TRAVERSED GROUND [bigmap]: known navigable, high confidence (layer `traversed`) + # small circle = lengths.radii.full_res (blue), large = lengths.radii.traversed_keep (orange) + # ====================================================================================== + traversed: + enabled: true # false: the robot's path is not remembered (no layer, no fine strip) + mode: persistent # circles: inside the small circle all at full resolution; between the two only + # the ground driven over; beyond the large one low resolution, and + # what was driven over there is FORGOTTEN (layer and saved maps too) + # persistent: inside the small circle all fine; outside it only the + # ground driven over, wherever it is, for good + + # ====================================================================================== + # 2.5 OBSTACLES [bigmap]: how what the sensors detect gets into the `obstacles` layer. + # In the order it is applied (the dependencies are checked at start and logged): + # detector -> memory -> removal -> (filters 2.7: inflation) -> tint + # A* reads `inflated_obstacles` if the inflation filter is in `filters`, else `obstacles`. + # Terrain steeper than terrain.max_slope_deg is always an obstacle, whatever is set here. + # ====================================================================================== + obstacles: + # WHO detects obstacles with the sensors (the only way that decides it): + # bigmap_dynamic bigmap's own dynamic map (2.6): heights relative to the robot, independent + # of the localization's z. Needs removal.enabled to reach the planner. + # navmap_filter the ObstacleFilter plugin of easynav_navmap_maps_manager: heights through + # the map -> robot TF (needs a good z). Needs it in `filters` (2.7); + # memory is off with it. + # (before: `source: dynamic | filter`, still accepted) + detector: bigmap_dynamic + removal: # [bigmap_dynamic] NavCels with a detection, and those within + enabled: true # lengths.radii.obstacle_clearance of one, are REMOVED from the + # published NavMap (holes in RViz) and set LETHAL in `obstacles`. + # false: detections only reach the dynamic map, NOT the planner + memory: # [bigmap_dynamic] layer `obstacle_hits` (uint8 per NavCel) + enabled: true # true: an obstacle is a detection that stays min_time over a NavCel, + # and the layer counts how often. The sensors are analysed even + # if dynamic_map.enabled is false. false: a detection blocks after + # dynamic_map.confirm_time and goes after forget_time (and nothing + # is analysed if dynamic_map.enabled is false). Runtime: + # ros2 param set /maps_manager_node bigmap.obstacles.memory.enabled false + # (a recovery that takes care of obstacles itself can do that): + # the fixed obstacles are released in that cycle; the counts are + # kept and come back when it is switched on again + min_time: 5.0 # s a detection must stay over the same NavCel (gaps over 1 s restart + # the count) to be an obstacle = one hit; one more per min_time s + # it goes on or comes back + max_hits: 3 # 1..255 hits that FIX the obstacle: blocked for good, seen or not + # (about max_hits x min_time s of detection). Below it the NavCel + # blocks only while detected. Steep terrain shows max_hits in the + # layer. 1: the first one is permanent. A fixed obstacle blocks + # only NavCels up to 4 x cell_size.min: far away it waits until the + # mesh there is fine + tint: # [bigmap] display only + enabled: true # paint the `obstacles` NavCels over the imagery in RViz + color: [255, 40, 40] # RGB + strength: 0.65 # 0 = imagery only, 1 = flat colour + + # ====================================================================================== + # 2.6 DYNAMIC MAP [bigmap]: what the sensors see above a NavCel raises it to that height. + # Heights are relative to the robot and referred to the terrain under it (not through the + # map -> robot TF, whose z drifts). Published at /bigmap/dynamic_map and NavState + # "map.dynamic"; the static map is untouched. Analysed if enabled OR obstacles.memory.enabled + # (only inside lengths.radii.sensor_range; the cost grows with that area). + # ====================================================================================== + dynamic_map: + enabled: false # true: analyse and publish the dynamic map (its detections block the planner too, + # after confirm_time, unless obstacles.memory.enabled). false: neither + # (the memory may still analyse the sensors) + # SIZE OF AN OBSTACLE: a point counts if it is at least min_height above the plane of the NavCel + # it falls in, along its normal, with the robot's roll/pitch/yaw taken into account: a ramp the + # robot drives up is the NavCel's own plane and never an obstacle. + min_height: 0.3 # m: lower = also small objects; higher = bumps and noise ignored + min_vertical_extent: 0.2 # m: ...and the points of one cell span at least this in height (flat + # ground the map puts a bit low is not an obstacle). Keep it above + # 0.25 m x tan(steepest slope) + confirm_time: 0.3 # s: ...and it is seen in the same cell this long (fleeting errors are + # not obstacles). Not used as the obstacle rule when memory is on + max_height: 3.0 # m above the robot's ground: higher points (ceilings, the floor above) ignored + forget_time: 5.0 # s a detection stays without being seen again + cell: 0.25 # m: detections are merged on this grid + voxel: 0.1 # m: sensor clouds are thinned to one point per voxel first (bigger = cheaper) + # GROUND MODEL FROM THE SENSOR: in a window in front of the robot a plane is fitted (RANSAC) to + # the lowest point of each cell: the ramp being climbed. Whatever lies on it is ground. + ground_fit: + enabled: true # false: only the map decides what is ground + window_length: 4.0 # m ahead of the robot + window_width: 3.0 # m across, centred on the robot + z_min: -1.0 # m (robot frame): lower points are not used for the plane + z_max: 2.5 # m: higher points are not used for the plane + tolerance: 0.08 # m: a lowest point this close to the plane is ground + max_tilt_deg: 40.0 # deg: a plane steeper than this is a wall, not ground + min_cells: 12 # fewer cells than this: only the map decides + + # ====================================================================================== + # 2.7 NAVMAP FILTERS [easynav_navmap_maps_manager]: plugins bigmap borrows, not its own. + # `filters` is the ONLY on/off switch of each of them: a filter runs if and only if it is + # listed here (and has `.plugin`). Instance names are free, except that `obstacles` + # is bigmap's own section (2.5): name an obstacle filter `obstacle_filter`. + # What a filter is, is told by the layer it writes: + # inflation -> inflated_obstacles independent of obstacles.detector + # obstacle_filter -> obstacles only with obstacles.detector: navmap_filter + # [inflation] inflation on, sensors by bigmap (default) + # [obstacle_filter, inflation] + detector: navmap_filter (obstacles.memory is then off) + # [] no inflation: A* reads `obstacles` and keeps no distance + # Listing obstacle_filter with detector bigmap_dynamic, or the reverse, is reported at start. + # ====================================================================================== + filters: [inflation] + inflation: + plugin: easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter + # NavCels within system_node.robot_geometry.inscribed_radius of an obstacle are blocked + # (shared: system_node defines it, this filter reads it); up to inflation_radius the cost + # decays and keeps the paths away from obstacles (and from the map border) + inflation_radius: 2.0 # m, >= system_node.robot_geometry.inscribed_radius + cost_scaling_factor: 1.5 # >0: how fast the cost decays with the distance + obstacle_filter: # only loaded if listed in `filters` + plugin: easynav_navmap_maps_manager/NavMapMapsManager/ObstaclesFilter + max_range: 10.0 # m: points farther than this from the robot are ignored + min_height: 0.1 # m above the NavMap surface under them: lower points are ground + downsample_resolution: 0.3 # m: voxel the points are reduced to first (<= 0: off) + + # ====================================================================================== + # 2.8 OUTPUT [bigmap] + # ====================================================================================== + output: + publish_markers: true # the two circles etc. as RViz markers + # RViz shows the map only up to this height (a click lands on a lower floor); runtime: + # ros2 param set /maps_manager_node bigmap.output.view_max_z 3.0 + # Only the displayed map: the planner's and the saved ones are complete. 1000 = everything. + view_max_z: 1000.0 # m + # Save the map as it is now (with the resolutions it has now) as a NavMap file: + # ros2 service call /maps_manager_node/bigmap/save_map std_srvs/srv/Trigger (all) + # ros2 service call /maps_manager_node/bigmap/save_traversed std_srvs/srv/Trigger (only the + # NavCels the robot has driven over). The reply says the file written. + save_directory: "~/.ros/bigmap/saved" + # Timing of the sections of the maps manager: every `period` s a table in the log with the + # mean / worst ms per call and the ms spent per second of each (the first rows cost most). + profile: + enabled: false # true: log that table + period: 5.0 # s + +# --------------------------------------------------------------------------------------------- +# 3. CONTROLLER [easynav_regulated_pp_controller]: Regulated Pure Pursuit (same in all scenarios). +# Nothing of bigmap's own is read here. +# --------------------------------------------------------------------------------------------- +controller_node: + ros__parameters: + use_sim_time: true + robot_limits: + max_linear_vel: 1.0 + min_linear_vel: -0.2 + max_angular_vel: 1.0 + max_linear_acc: 0.5 + max_linear_decel: 1.0 + max_angular_acc: 1.0 + max_angular_decel: 2.0 + controller_types: [regulated] + regulated: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Area where obstacles regulate the velocity. + safety_margin: 0.1 + use_dynamic_window: false + allow_reversing: false + # Lookahead, scaled for the Summit XL + lookahead_dist: 1.0 + min_lookahead_dist: 0.5 + max_lookahead_dist: 1.5 + lookahead_time: 1.5 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 0.8 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 1.5 + regulated_linear_scaling_min_speed: 0.2 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.5 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 1.0 + z_min_filter: 0.3 # ignore the ground seen by the 3D lidar and the depth camera + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 1.0 + # Goal tolerances (system_node: 0.3 m / 0.15 rad) + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.15 + +# --------------------------------------------------------------------------------------------- +# 4. LOCALIZER [OUTDOOR] [easynav_fusion_localizer]: GPS + wheel odometry + IMU, UKF (FusionLocalizer) +# --------------------------------------------------------------------------------------------- +localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [ukf] + ukf: + rt_freq: 50.0 + freq: 5.0 + plugin: easynav_fusion_localizer/FusionLocalizer + # Same origin as gps_origin above: map (0, 0) is the spawn. The launch overrides these three + # with the exact origin of the exported Gazebo world (easynav_overrides.yaml, generated). + latitude_origin: 40.284796 + longitude_origin: -3.821407 + altitude_origin: 0.0 + # Global filter only: map -> odom from GPS, wheel odometry and IMU heading. + # odom -> base_footprint is the wheel odometry's own TF (diff drive controller). + global_filter: + two_d_mode: false # 3D: the robot climbs the terrain, so z (and roll/pitch) matter + # Without this the filter never ticks its frequency diagnostic, which then reports + # "odometry/filtered: no events recorded, 0 Hz" even when it works + print_diagnostics: true + # The filter is stepped at the plugin's rt_freq (50 Hz): that is the rate its frequency + # diagnostic should expect (the default, 30 Hz, makes it warn "frequency too high"). The + # sensor timeout defaults to 1 / frequency, so it is kept at what it was with 30 Hz. + frequency: 50.0 + sensor_timeout: 0.0333 + publish_tf: true + transform_timeout: 0.1 + map_frame: map + odom_frame: odom + base_link_frame: base_footprint + world_frame: map + gps0: /gps/fix + gps0_config: [true, true, true, false, false, false, + false, false, false, false, false, false, + false, false, false] + gps0_differential: false + odom0: /robotnik_base_control/odom + odom0_config: [false, false, false, false, false, false, + true, true, true, false, false, true, + false, false, false] + odom0_differential: false + imu0: /imu/data + imu0_config: [false, false, false, true, true, true, + false, false, false, false, false, true, + false, false, false] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + +# --------------------------------------------------------------------------------------------- +# 5. PLANNER [easynav_navmap_planner]: A* over the NavMap (same in all scenarios). +# Cost layer: `inflated_obstacles` if bigmap.filters has the inflation, else `obstacles`. +# --------------------------------------------------------------------------------------------- +planner_node: + ros__parameters: + use_sim_time: true + planner_types: [astar] + astar: + freq: 1.0 + plugin: easynav_navmap_planner/AStarPlanner + # the map changes with the robot's position (level of detail): keep the path being followed + # while its first replan_hysteresis_horizon m are traversable, unless a new one is 20 % + # shorter (0: always the new one; horizon 0: the whole path is checked) + replan_hysteresis: 0.2 + replan_hysteresis_horizon: 20.0 # m, ~ bigmap full_res: beyond it the map is coarse and changes + +# --------------------------------------------------------------------------------------------- +# 6. SENSORS [easynav_sensors] [OUTDOOR]: lidar + IMU + GPS (the camera is declared but not used) +# --------------------------------------------------------------------------------------------- +sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [laser, imu, gps] + perception_default_frame: odom + laser: + topic: front_laser/points + type: sensor_msgs/msg/PointCloud2 + camera: + topic: front_camera/depth/color/points + type: sensor_msgs/msg/PointCloud2 + imu: + topic: imu/data + type: sensor_msgs/msg/Imu + gps: + topic: gps/fix + type: sensor_msgs/msg/NavSatFix + group: gnss # the FusionLocalizer reads the GNSS perceptions from this group + +# --------------------------------------------------------------------------------------------- +# 7. SYSTEM [easynav_system] +# --------------------------------------------------------------------------------------------- +system_node: + ros__parameters: + use_sim_time: true + use_real_time: true + rt_freq: 50.0 + position_tolerance: 0.3 + angle_tolerance: 0.15 + robot_geometry: # SHARED: read by every plugin; bigmap.inflation.inflation_radius >= inscribed_radius + radius: 0.7 # m + inscribed_radius: 0.7 # m: NavCels closer than this to an obstacle are blocked + height: 1.0 # m diff --git a/maps_managers/easynav_bigmap_maps_manager/easynav_bigmap_maps_manager_plugins.xml b/maps_managers/easynav_bigmap_maps_manager/easynav_bigmap_maps_manager_plugins.xml new file mode 100644 index 00000000..7dc21066 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/easynav_bigmap_maps_manager_plugins.xml @@ -0,0 +1,9 @@ + + + + + Maps manager for large terrains: a NavMap whose triangle size grows with the distance to the robot (RTIN dynamic LOD). + + + + diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/BigMapMapsManager.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/BigMapMapsManager.hpp new file mode 100644 index 00000000..27395a6c --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/BigMapMapsManager.hpp @@ -0,0 +1,281 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__BIGMAPMAPSMANAGER_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__BIGMAPMAPSMANAGER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" +#include "navmap_ros_interfaces/msg/nav_map.hpp" +#include "navmap_ros_interfaces/msg/nav_map_layer.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "easynav_core/MapsManagerBase.hpp" +#include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" +#include "pluginlib/class_loader.hpp" +#include "easynav_bigmap_maps_manager/dynamic_layer.hpp" +#include "easynav_bigmap_maps_manager/obstacle_memory.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include "easynav_bigmap_maps_manager/known_map.hpp" +#include "easynav_bigmap_maps_manager/profiler.hpp" +#include "easynav_bigmap_maps_manager/update_watchdog.hpp" +#include "easynav_bigmap_maps_manager/navmap_dem.hpp" +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" +#include "easynav_bigmap_maps_manager/surface_lod.hpp" + +namespace easynav +{ +namespace bigmap +{ + +/** + * @class BigMapMapsManager + * @brief EasyNav MapsManagerBase plugin ("bigmap"): a NavMap of a large terrain or building whose + * triangle size grows with the distance to the robot. + * + * WHAT IT OWNS (parameters under `maps_manager_node.bigmap`, documented in bigmap_bringup/config): + * terrain, multilevel, lengths, lod, traversed, obstacles (detector, removal, memory, tint), + * dynamic_map, output. It publishes `/bigmap/map` (NavMap), `.../markers`, `.../dynamic_map` + * and sets "map.navmap" (and "map.dynamic") in the NavState. + * + * WHAT IT BORROWS: the NavMapFilter plugins of easynav_navmap_maps_manager, loaded through the + * `filters` list (see below), and `system_node.robot_geometry` (read by the inflation filter). + * + * Every update it takes the robot position and re-meshes when the robot moved `lengths.remesh_step`: + * - grid engine (outdoors, any single-floor NavMap): RTIN over a height grid (LodMesher); + * - hierarchy engine (`multilevel.enabled`): clustering of the NavMap's own triangles + * (SurfaceLodMesher), for floors over each other and ramps. + * The same rule decides split and merge in both (lod_policy.hpp). The NavMap in the NavState has: + * - "occupancy" (uint8): the static map, LETHAL_OBSTACLE on NavCels steeper than `max_slope_deg`; + * - "obstacles": occupancy plus what the sensors detect (who writes that: `obstacles.detector`); + * - "inflated_obstacles": written by the inflation filter if it is in `filters`; + * - "obstacle_hits": how many times an obstacle was detected on each NavCel (`obstacles.memory`); + * - "traversed", "elevation", "lod_size"; vertex colours if the map has imagery. + * + * OBSTACLES, in the order they are applied (their dependencies are checked at start, see + * pipeline_check.hpp, and the result is logged): + * 1. detector `bigmap_dynamic` (default): the DynamicLayer finds sensor points above a NavCel + * (heights relative to the robot: independent of the localization's z). Detector + * `navmap_filter`: the ObstacleFilter plugin does (it measures through the map -> robot TF + * and needs a good z); it must be in `filters`, under an instance name other than `obstacles`. + * 2. `obstacles.memory` (bigmap_dynamic only): a detection that lasts `min_time` over a NavCel is + * an obstacle and one hit; `max_hits` hits fix it for good. + * 3. `obstacles.removal` (bigmap_dynamic: it is what puts the detections in "obstacles"): + * those NavCels, and the ones within `lengths.radii.obstacle_clearance`, are removed from the + * published NavMap and set LETHAL for the planner. + * 4. `filters`: each listed filter runs, and ONLY the listed ones (the list is their on/off + * switch). The inflation filter spreads the cost around LETHAL NavCels; A* reads + * "inflated_obstacles" if it exists, else "obstacles". + * 5. `obstacles.tint`: paints the "obstacles" NavCels in RViz (display only). + * The dynamic map (`dynamic_map`) raises the NavCels where the sensors saw something and publishes + * them; it is analysed if `dynamic_map.enabled` or `obstacles.memory.enabled`. + * + * RELIABILITY AND LATENCY: + * - `lod.hole_error` / `lod.observed`: the holes of the map (its obstacles) keep their shape away + * from the robot, by an error on their border and by keeping at full resolution the borders the + * robot has seen from near (so a far plan does not go through an obstacle it has already seen, + * and the robot does not come back and forth between two meshes). + * - `recovery`: with a goal and no path for some cycles, the full-resolution circle is doubled + * (up to a cap) and the map re-meshed, until the goal is reached or dropped. + * - `lod.async_remesh`: the re-mesh runs in a worker thread and the cycle that asked for it does + * not wait: the previous map is used until the new one is ready. + * + * Parameter names used before the reorganisation are still read, with a warning (param_any). + */ +class BigMapMapsManager : public easynav::MapsManagerBase +{ +public: + BigMapMapsManager() = default; + void on_initialize() override; + +protected: + void update(::easynav::NavState & nav_state) override; + +private: + bool robot_position( + const ::easynav::NavState & nav_state, double & x, double & y, double & z, + std::array & q_xyzw); + // Re-meshing: prepare (driven-over / observed ground, in this thread), make (extraction and + // NavMap: in this thread or in the worker) and commit (in this thread) + struct Remesh + { + bigmap_maps_manager::LodMesh mesh; + ::navmap::NavMap navmap; + double x{0.0}, y{0.0}, z{0.0}; + std::chrono::steady_clock::time_point t0; + }; + void prepare_remesh(double x, double y, double z); + Remesh make_remesh( + double x, double y, double z, const bigmap_maps_manager::LodParams & params, + bigmap_maps_manager::Profiler * prof) const; + void commit_remesh(Remesh && r); + void mark_observed(double x, double y, double z); // the obstacle borders seen from near + void mark_trail(double x, double y, double z); + void check_recovery(const ::easynav::NavState & nav_state); + bool is_traversed(const bigmap_maps_manager::LodMesh & mesh, std::size_t t) const; + ::navmap::NavMap build_navmap(const bigmap_maps_manager::LodMesh & mesh) const; + + // Obstacle removal: NavCels with sensor obstacles (and those within a radius) are cut out of the + // published NavMap and blocked (LETHAL) for the planner + void restore_removed(::easynav::NavState & nav_state); // our marks -> base, before the filter + // Dynamic map: NavCels raised to what the sensors measure (see DynamicLayer) + void update_dynamic_map( + ::easynav::NavState & nav_state, double x, double y, double z, + const std::array & q_xyzw); + navmap_ros_interfaces::msg::NavMap make_dynamic_msg( + const std::vector & tiles, const rclcpp::Time & stamp) const; + // Obstacle memory (layer `obstacle_hits`): how many times an obstacle has been detected on each + // NavCel, from the hits of ObstacleMemory (which outlive the re-meshes) + bool memory_active() const; // enabled now (runtime parameter obstacles.memory.enabled) + void write_hits(::navmap::NavMap & nm) const; // the memory's counts onto the NavCels + void reset_hits(::navmap::NavMap & nm) const; // the layer without the memory (steep only) + void fixed_seeds(std::vector & seeds) const; // NavCels of fixed obstacles + // nothing analyses the sensors any more: what they blocked is released + void release_sensor_obstacles(::easynav::NavState & nav_state); + // every NavCel of the current mesh inside a memory cell (its square metre, at its height) + void triangles_of( + const bigmap_maps_manager::ObstacleMemory::Cell & cell, + std::vector & out) const; + void apply_obstacle_removal(::easynav::NavState & nav_state); // seeds: the ObstaclesFilter's + void apply_removal(::easynav::NavState & nav_state, const std::vector & seeds); + navmap_ros_interfaces::msg::NavMap make_map_msg( + const std::vector & removed, const ::navmap::NavMap & layers_from, + const rclcpp::Time & stamp, bool tint = true) const; + void publish_map(const std::vector & removed, const ::navmap::NavMap & layers_from); + + // Saving the map as it is (`all`, or only the `traversed` NavCels), asked for through services + // and done by update(), the thread that owns the mesh + struct SaveRequest + { + bool traversed_only{false}; + std::string path; // decided when asked, written by the next update() + }; + void process_save(::easynav::NavState & nav_state); + void create_services(const std::string & base); // save_map, save_traversed, clear_obstacles + std::mutex save_mutex_; + std::shared_ptr pending_save_; + std::string save_directory_; + rclcpp::Service::SharedPtr save_all_srv_, save_traversed_srv_; + /// /clear_obstacles: forgets every obstacle of the memory (queued, done by update()) + rclcpp::Service::SharedPtr clear_obstacles_srv_; + std::atomic clear_obstacles_requested_{false}; + + std::unique_ptr mesher_; + // Multi-level mode (`multilevel.enabled`): the NavMap's own triangles are simplified by + // clustering, instead of a height grid with RTIN (floors over each other, ramps, any building) + std::unique_ptr surface_; + bool has_known() const; // driven-over ground exists (either mode) + // What RViz shows (runtime parameter `view_max_z`): NavCels above this height are left out of the + // published map, so a click lands on a lower floor. The planner's map is not touched. + double view_max_z_{1e9}; + // Timing of the sections of update(), logged every profile.period s when profile.enabled + bigmap_maps_manager::Profiler profiler_; + bool profile_enabled_{false}; + // Says (on the wall clock, from the non-real-time executor) when update() stops being called: + // EasyNav calls it when the node's ROS clock says it is due, so a clock that never moves is + // otherwise total silence (the throttled logs are throttled on that same clock) + bigmap_maps_manager::UpdateWatchdog watchdog_; + rclcpp::TimerBase::SharedPtr watchdog_timer_; + void check_watchdog(); + bigmap_maps_manager::Profiler * prof() {return profile_enabled_ ? &profiler_ : nullptr;} + bigmap_maps_manager::MapOrigin origin_; // where the map's (0, 0, 0) is, if anywhere + std::unique_ptr known_; // ground the robot has driven over + // obstacle borders (holes of the map) the robot has seen at full resolution (grid engine) + std::unique_ptr observed_; + bool observed_enabled_{true}; + double observed_mark_radius_{10.0}; + // no path to the goal for recovery_cycles_ cycles: full_res doubled up to recovery_max_factor_ + bool recovery_enabled_{true}; + int recovery_cycles_{3}; + double recovery_max_factor_{4.0}; + int no_path_cycles_{0}; + double base_high_res_{10.0}, base_low_res_{200.0}; + bool force_remesh_{false}; + bool async_remesh_{false}; + std::vector> pending_marks_; // poses met while the worker meshes + bool trail_enabled_{true}; + double trail_mark_radius_{1.0}; + bool trail_persistent_{false}; // mode `persistent`; else `circles` + double trail_keep_radius_{40.0}; // the larger circle (mode `circles`) + bigmap_maps_manager::LodParams params_; + double hysteresis_{2.0}; + double max_slope_deg_{35.0}; + bool publish_markers_{true}; + std::string map_frame_; + + bool dynamic_enabled_{true}; + bigmap_maps_manager::DynamicParams dyn_; + bigmap_maps_manager::DynamicLayer dyn_layer_; + std::unique_ptr locator_; // of mesh_ + bool dyn_dirty_{false}; + double last_dyn_publish_{0.0}; + rclcpp::Publisher::SharedPtr dynamic_pub_; + + // Memory of where obstacles were detected and how often (obstacles.memory.*) + std::unique_ptr memory_; + double memory_min_time_{5.0}; + int memory_max_hits_{3}; + bool memory_was_on_{false}; + + bool obstacles_from_filter_{false}; // else: from the dynamic map + bool removal_enabled_{true}; + double removal_radius_{1.0}; + bool tint_enabled_{true}; // obstacle NavCels are painted over the imagery + std::array tint_rgb_{255.f, 40.f, 40.f}; + float tint_strength_{0.65f}; + bigmap_maps_manager::LodMesh mesh_; // the mesh behind navmap_ (same triangle order) + std::vector centroid_x_, centroid_y_, centroid_z_; + // the centroids by cells of centroid_cell_ (>= the clearance radius), for the ring of removal + std::unordered_map> centroid_grid_; + double centroid_cell_{1.0}; + std::vector removed_; // NavCels currently removed (sorted) + std::vector pending_flags_; // removed NavCels of the map published at the end + bool publish_pending_{false}; + + std::optional> last_meshed_; + double last_meshed_z_{0.0}; + ::navmap::NavMap navmap_; + bool map_changed_{false}; + + std::unique_ptr> filters_loader_; + std::vector> filters_; + float map_resolution_{1.0f}; + std::size_t last_subscribers_{0}; + + rclcpp::Publisher::SharedPtr layer_updates_pub_; + rclcpp::Publisher::SharedPtr navmap_pub_; + rclcpp::Publisher::SharedPtr markers_pub_; + + // the re-mesh running in the worker (lod.async_remesh). Last member: destroyed first, its + // destructor waits for the worker, which reads the members above + std::future remesh_job_; +}; + +} // namespace bigmap +} // namespace easynav + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__BIGMAPMAPSMANAGER_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/dem_grid.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/dem_grid.hpp new file mode 100644 index 00000000..285c2248 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/dem_grid.hpp @@ -0,0 +1,76 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__DEM_GRID_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__DEM_GRID_HPP_ + +#include +#include +#include +#include +#include + +namespace bigmap_maps_manager +{ + +/// Square regular height grid in the local ENU frame, centred on the GPS origin: the DEM file the +/// plugin reads (terrain.dem_file). It is made by navmap_tools' navmap_gis_tool (--dem-out), which +/// writes the format of save() below (geo/dem_file.py there); test/data/dem_golden.bin keeps both +/// sides honest. +/// Vertex (i, j) sits at x = (i - c) * resolution, y = (j - c) * resolution, c = (side - 1) / 2, +/// so the origin is an exact node. z is relative to `reference_altitude`. +struct DemGrid +{ + /// where the heights come from (0 was Google's) + enum Source {kSynthetic = 1, kIgn = 2, kMesh = 3, kTerrainTiles = 4, kCopernicus = 5}; + + int side{0}; ///< nodes per axis (always odd) + double resolution{0.0}; ///< metres between nodes of this grid (the mesh resolution) + double data_resolution{0.0}; ///< spacing at which the source was sampled (0: = resolution) + double radius{0.0}; ///< requested half-side (m) + double origin_lat{0.0}; + double origin_lon{0.0}; + /// Altitude as configured (NaN = automatic, i.e. DEM value at the origin). + double requested_reference{std::numeric_limits::quiet_NaN()}; + double reference_altitude{0.0}; ///< metres ASL mapped to z = 0 + int source{kIgn}; + int imagery_zoom{0}; ///< zoom of the imagery in `rgb` (0 = none) + std::vector rgb; ///< optional imagery colour per node, 3 bytes, same order as z + std::vector z; ///< row-major, index = j * side + i + /// Optional: which cells have ground, (side - 1)^2 bytes, cell (i, j) between nodes i..i+1 and + /// j..j+1, 1 = ground, 0 = a hole (a building, a wall: something the robot cannot be on). Empty: + /// every cell has ground. A file without it (older) reads as empty. + std::vector cell_valid; + + float at(int i, int j) const {return z[static_cast(j) * side + i];} + double x(int i) const {return (i - (side - 1) / 2) * resolution;} + double y(int j) const {return (j - (side - 1) / 2) * resolution;} + + /// Terrain height (m, relative to the reference altitude) at metric (x, y): bilinear, clamped to + /// the grid. + double height_at(double x, double y) const; + + /// Binary file. load() returns nullopt if missing/corrupt. + bool save(const std::string & path) const; + static std::optional load(const std::string & path); + + /// A grid with every `stride`-th node of this one ((side - 1) must be divisible by stride). + /// The imagery colours are not subsampled (empty rgb). A coarse cell has ground only if all its + /// cells do (a hole is never closed). + DemGrid subsample(int stride) const; +}; + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__DEM_GRID_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/dynamic_layer.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/dynamic_layer.hpp new file mode 100644 index 00000000..fdb6abe6 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/dynamic_layer.hpp @@ -0,0 +1,167 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__DYNAMIC_LAYER_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__DYNAMIC_LAYER_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" + +namespace bigmap_maps_manager +{ + +/// Finds the triangle of a LodMesh that contains a point (x, y). Built once per mesh. +class TriangleLocator +{ +public: + explicit TriangleLocator(const LodMesh & mesh, double cell = 8.0); + /// Index of the triangle containing (x, y), or -1. + int locate(double x, double y) const; + /// With several surfaces over (x, y) (floors of a building): the highest one that is not above + /// `z` + 0.25 m, or the lowest if all are. NaN `z`: the first one found. + int locate_at(double x, double y, double z) const; + +private: + const LodMesh * mesh_; + double cell_, x0_, y0_; + int nx_, ny_; + std::vector> bins_; +}; + +struct DynamicParams +{ + double max_range{5.0}; ///< only sensor points this close to the robot (m) + /// A point must be this far above the plane of the NavCel it falls in, measured along that + /// NavCel's normal (m): a ramp, however steep, is the NavCel's own plane and never counts. + double min_height{0.3}; + /// ... and the points of its cell must span at least this much in height. An object has + /// vertical extent; ground that the terrain model puts a few decimetres too low (the DEM is never + /// exact) is a flat surface and spans only what its slope gives within one cell, so it is not + /// mistaken for an obstacle however far it is above the model. + double min_vertical_extent{0.2}; + /// A detection counts only once it has been seen in the same cell for this long (s). An object + /// stays where it is; a fleeting error (the robot's attitude changing faster than the sensors + /// and the IMU agree on) moves to other cells in the next frame and never gets confirmed. + double confirm_time{0.3}; + double max_height{3.0}; ///< ... and no higher than this above the robot's ground (m) + double forget_time{5.0}; ///< a detection not seen again for this long is dropped (s) + double cell{0.25}; ///< detections are merged on a grid of this size (m) + double voxel{0.1}; ///< sensor clouds are thinned to one point per voxel this big (m) + + /// Local ground model from the sensor itself. In a window in front of the robot (window_length + /// ahead, window_width across, in the robot's frame) the lowest point of every cell is taken + /// and a plane is fitted to them (RANSAC, tilt limited so a wall can't be the plane). A cell + /// inside the window is an obstacle only if its highest point also rises min_height above THAT + /// plane (along its normal): a ramp, up or down, is a plane and its points are all on it, + /// whatever the map says (ground_tolerance decides which lowest points fit the plane). Outside + /// the window, or with fewer than ground_min_cells cells, only the map is used. + bool ground_fit{true}; + double window_length{4.0}; ///< m ahead of the robot + double window_width{3.0}; ///< m across (centred on the robot) + double window_z_min{-1.0}; ///< body-frame z range of the points that can be ground (m) + double window_z_max{2.5}; + double ground_tolerance{0.08}; ///< a cell's lowest point within this of the plane is ground (m) + double max_ground_tilt_deg{40.0}; + int ground_min_cells{12}; +}; + +/// A NavCel raised to what the sensor measured. +struct RaisedTile +{ + std::uint32_t triangle; ///< index in the LodMesh + float z_top; ///< height of the NavCel now (map frame, relative to the DEM reference) + float above_surface; ///< z_top minus the mean terrain height of the NavCel +}; + +/// A detection that has gone on for another `min_time` seconds (see DynamicLayer::sustained) +struct SustainedHit +{ + float x, y; ///< map frame (m) + float z; ///< highest point seen in the cell + float surface; ///< terrain height under it +}; + +/// Memory of what the sensors saw above the terrain, to raise the NavCels where it was. +/// +/// Heights are measured relative to the robot and referred to the terrain under it +/// (`z = terrain(robot) + z of the point rotated by the robot's orientation`), not through the +/// map -> robot TF: the localization's z drifts by metres, while the robot's x, y and its +/// roll/pitch/yaw (from the IMU) are what is reliable. +class DynamicLayer +{ +public: + using Surface = std::function; ///< terrain height at (x, y) + + /// `points`: sensor points in the robot's body frame (x forward, z up, z = 0 at the ground + /// contact), which tilts with the robot. `q_xyzw`: the robot's full orientation in the map + /// (roll, pitch and yaw): on a slope the sensors tilt with it, and ignoring that would turn the + /// slope into obstacles. `now`: seconds. Returns true if the detections changed noticeably. + /// + /// With `mesh` and `locator` (those of the NavMap in use) the height of a point is measured + /// against the plane of the NavCel it falls in, along its normal; points outside the mesh are + /// ignored. Without them `terrain` is used and the height is the vertical one. + bool ingest( + const std::vector> & points, double robot_x, double robot_y, + const std::array & q_xyzw, double now, const Surface & terrain, + const DynamicParams & p, const LodMesh * mesh = nullptr, + const TriangleLocator * locator = nullptr, + double robot_z = std::numeric_limits::quiet_NaN()); + + /// The same for a robot that is level, with this heading. + bool ingest( + const std::vector> & points, double robot_x, double robot_y, + double robot_yaw, double now, const Surface & terrain, const DynamicParams & p, + const LodMesh * mesh = nullptr, const TriangleLocator * locator = nullptr); + + /// Drops detections not seen for p.forget_time (unconfirmed ones: for 0.5 s) and confirms those + /// seen for p.confirm_time. Returns true if the confirmed set changed. + bool expire(double now, const DynamicParams & p); + + /// The NavCels that contain confirmed detections, raised to the highest one seen in each. With + /// `min_age` only detections that have been seen without a break (gaps over 1 s start the count + /// again) for at least that long (s). + std::vector tiles( + const LodMesh & mesh, const TriangleLocator & locator, double min_age = 0.0) const; + + /// One SustainedHit per detection cell for every `min_time` seconds it has been seen without a + /// break since the last call: 5 s over the same spot is one hit, 10 s two (reported by two + /// calls). Detections are cells of `DynamicParams::cell`: the caller merges those of one NavCel. + std::vector sustained(double now, double min_time); + + std::size_t size() const {return entries_.size();} + void clear() {entries_.clear();} + +private: + struct Entry + { + float x, y, z, surface; + double stamp; ///< last time it was seen + double first; ///< first time it was seen + bool confirmed; + double streak; ///< start of the current run seen without a gap over 1 s + int periods; ///< `min_time` periods of that run already reported by sustained() + }; + std::unordered_map entries_; +}; + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__DYNAMIC_LAYER_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/known_map.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/known_map.hpp new file mode 100644 index 00000000..d2e8e5e1 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/known_map.hpp @@ -0,0 +1,102 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__KNOWN_MAP_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__KNOWN_MAP_HPP_ + +#include +#include +#include + +namespace bigmap_maps_manager +{ + +/// The DEM nodes the robot has driven over: ground it has proved navigable. The mesher keeps +/// the NavCels there at full resolution, however far the robot is now. +/// +/// distance() is the exact Euclidean distance to the nearest traversed node: a 1-Lipschitz +/// function of position, which is all the crack-free argument of the LOD policy needs. It is +/// answered from a 2-D tree of the traversed nodes (built by update()), not from a distance +/// field of the whole grid: the mesher asks for a few tens of thousands of nodes per re-mesh. +class KnownMap +{ +public: + /// `side` x `side` DEM nodes, `resolution` m apart, node (side-1)/2 at the origin. Only those + /// nodes can be marked; distance() is exact for any node, the padding of the RTIN tree (2^k + 1) + /// included, which keeps it 1-Lipschitz there too (the crack-free argument needs it). `grid`: + /// unused (it sized the marks for the padding; kept for source compatibility). + KnownMap(int side, double resolution, int grid = 0); + + /// Marks the nodes within `radius` m of (x, y) (local frame, metres). Returns how many were new. + int mark(double x, double y, double radius); + + /// Marks one node. Returns true if it was new. + bool mark_node(int ix, int iy); + + /// Recomputes the distance field from all the traversed nodes, if something was marked since + /// the last call. + void update(); + + /// The same, but only the traversed nodes within `radius` m of (x, y) count: ground the robot + /// drove over farther away than that is forgotten as far as resolution goes (it stays marked). + void update_within(double x, double y, double radius); + + /// Forgets (unmarks) the traversed nodes farther than `radius` m from (x, y): they are ground + /// the robot has not driven over as far as the map is concerned, until it drives over it again. + /// Returns how many were forgotten. + std::size_t forget_outside(double x, double y, double radius); + + /// True if a traversed node lies within `reach` nodes (Chebyshev) of node (ix, iy). + bool near_marked(int ix, int iy, int reach) const; + + /// Nodes the distance field is currently measured from. + std::size_t sources() const {return sources_;} + + bool empty() const {return count_ == 0;} + std::size_t count() const {return count_;} + + /// Distance (m) from node (ix, iy) to the nearest traversed node (any indices, also beyond the + /// grid). Infinite when nothing was traversed. Call update() first. + double distance(int ix, int iy) const; + + /// True if the node (ix, iy) itself was traversed. + bool known(int ix, int iy) const; + +private: + int side_; // nodes per axis of the field (the DEM's, or the padded grid's) + double res_; + int center_; + // read by the save services from another thread (has_known): atomic, so that the read is + // well defined (the mesher's thread is the only writer) + std::atomic count_{0}; + std::size_t sources_{0}; + std::vector nodes_; // marked nodes, in the order they were marked + void compute(const std::vector & sources); + bool dirty_{false}; + std::vector mark_; + // the sources as a balanced 2-D tree over this array (see compute): the node of a range + // [lo, hi) is its middle element, split along x or y + struct Node + { + int x, y; + bool by_x{true}; + }; + std::vector tree_; + void build(std::size_t lo, std::size_t hi); + void nearest(std::size_t lo, std::size_t hi, int x, int y, double & best) const; +}; + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__KNOWN_MAP_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/lod_mesher.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/lod_mesher.hpp new file mode 100644 index 00000000..eb93b5fc --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/lod_mesher.hpp @@ -0,0 +1,99 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__LOD_MESHER_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__LOD_MESHER_HPP_ + +#include +#include + +#include "easynav_bigmap_maps_manager/dem_grid.hpp" +#include "easynav_bigmap_maps_manager/known_map.hpp" +#include "easynav_bigmap_maps_manager/lod_policy.hpp" +#include "easynav_bigmap_maps_manager/rtin_error_map.hpp" + +namespace bigmap_maps_manager +{ + +/// Adaptive triangle mesh in the local ENU frame. Triangles are CCW seen from +z. +struct LodMesh +{ + std::vector x, y, z; ///< shared vertices (all of them are DEM nodes) + std::vector r, g, b; ///< per-vertex imagery colour (empty if the DEM has none) + std::vector tri; ///< 3 vertex indices per triangle + std::vector leg; ///< leg length (m) of each triangle, i.e. its LOD size + /// Per triangle, 1 on ground the robot has driven over (only the surface mesher fills it) + std::vector traversed; + /// Per triangle, the steepest slope (degrees) of the source triangles it stands for (only the + /// surface mesher fills it): what can be driven is decided by the data, not by the simplified + /// geometry, which clustering makes steeper than it is + std::vector slope; + std::size_t triangle_count() const {return tri.size() / 3;} +}; + +/// Extracts a crack-free adaptive mesh centred on a robot position. extract() is const but NOT +/// thread-safe: one extraction at a time. +class LodMesher +{ +public: + /// `valid`: optional, one byte per DEM node (same order as dem.z): 0 = no data. Without + /// `cell_valid`, NavCels whose centre is on a no-data node are not emitted. + /// `pinned`: optional, one byte per DEM node: 1 = narrow passage, always at the grid's full + /// resolution (see narrow_passage_nodes()). + /// `cell_valid`: optional, one byte per DEM cell ((side - 1)^2, cell (i, j) between nodes i..i+1 + /// and j..j+1): 1 = the source map has ground at the cell's centre. With it, a NavCel is + /// emitted only if the cell under its centroid has ground: at full resolution the holes of the + /// source (its obstacles) are exactly where they are, whatever their size (the node rule lost + /// every hole narrower than three cells). + /// `hole_error` (m, with `cell_valid`): the border of every hole counts as a height error of that + /// much (see RtinErrorMap), so holes keep their shape as far as the tolerance stays below it. + /// The DEM is taken by value: move it in (a big grid is not copied). + explicit LodMesher( + DemGrid dem, std::vector valid = {}, + const std::vector & pinned = {}, std::vector cell_valid = {}, + double hole_error = 0.0); + + LodMesher(const LodMesher &) = delete; + LodMesher & operator=(const LodMesher &) = delete; + + /// With `known` (distance field up to date) the ground it marks as traversed stays at full + /// resolution: the policy's distance becomes the smaller of the robot's and the traversed set's. + /// `observed` (optional, same rule): obstacle borders the robot has seen at full resolution, + /// kept so (a hole seen from near does not close again when the robot goes away, which would + /// let a plan through it and make the robot come back: an oscillation). + LodMesh extract( + double robot_x, double robot_y, const LodParams & params, + const KnownMap * known = nullptr, const KnownMap * observed = nullptr) const; + + const DemGrid & dem() const {return dem_;} + const RtinErrorMap & error_map() const {return errors_;} + /// Per DEM cell, 1 = ground (empty: no cell mask) + const std::vector & cell_valid() const {return cell_valid_;} + /// Bytes held by the mesher (the DEM, the error map, the masks) + std::size_t memory_bytes() const; + +private: + DemGrid dem_; + RtinErrorMap errors_; + std::vector valid_; + std::vector cell_valid_; +}; + +/// Nodes on the border of a hole: some of their (up to four) cells have ground and some do not. +/// One byte per DEM node; empty if `cell_valid` is. +std::vector hole_border_nodes(int side, const std::vector & cell_valid); + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__LOD_MESHER_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/lod_policy.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/lod_policy.hpp new file mode 100644 index 00000000..0a0c5982 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/lod_policy.hpp @@ -0,0 +1,117 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__LOD_POLICY_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__LOD_POLICY_HPP_ + +#include +#include + +namespace bigmap_maps_manager +{ + +/// Distance-driven LOD policy (all lengths in metres). +/// +/// A triangle is split when `leg > min_size` and (`error > tolerance(d)` or `leg > size_cap(d)`), +/// where d is the *effective* distance from the robot to the triangle's split vertex. +/// +/// Crack-freeness: RTIN is T-junction free iff "split(child vertex) => split(parent vertex)". +/// That holds if tolerance() and size_cap() are non-decreasing in d AND d is monotone along the +/// vertex hierarchy. The raw distance is not (a parent can be farther than its child), so we use +/// d_eff = max(0, dist - kReach * hypotenuse): the child's bounding disc is nested in the +/// parent's one for kReach >= (1 + sqrt(2)) / 2 ~= 1.207, hence d_eff(parent) <= d_eff(child). +struct LodParams +{ + double high_res_radius{10.0}; ///< full resolution (min_size leaves) while d_eff <= this + double low_res_distance{200.0}; ///< from this d_eff on, leaves may be as large as max_size + double min_size{1.0}; ///< smallest leaf leg (clamped by the DEM resolution) + double max_size{32.0}; ///< largest leaf leg anywhere + double base_error{0.02}; ///< height tolerance (m) inside the high-res disc + double error_growth{0.05}; ///< extra tolerance per metre beyond high_res_radius + /// Ground the robot has driven over (KnownMap) keeps full resolution up to this distance from + /// the traversed nodes, wherever the robot is now. + double known_margin{1.0}; + /// Beyond known_margin, resolution fades `known_falloff` times faster with the distance to the + /// traversed ground than it does with the distance to the robot (1: the same). + double known_falloff{1.0}; + /// Obstacle borders the robot has seen from near (observed) keep full resolution up to this + /// distance, with the same falloff beyond. Negative: known_margin. + double observed_margin{-1.0}; +}; + +constexpr double kReach = 1.25; + +inline double effective_distance(double dist_to_vertex, double hypotenuse) +{ + return std::max(0.0, dist_to_vertex - kReach * hypotenuse); +} + +inline double tolerance(const LodParams & p, double d_eff) +{ + return p.base_error + p.error_growth * std::max(0.0, d_eff - p.high_res_radius); +} + +/// Largest leaf leg allowed at effective distance d: min_size up to high_res_radius, max_size from +/// low_res_distance on, and in between geometric (log-linear): the cell size is multiplied by the +/// same factor for every metre of distance, so detail fades smoothly. Non-decreasing in d. +inline double size_cap(const LodParams & p, double d_eff) +{ + if (d_eff <= p.high_res_radius) { + return p.min_size; + } + if (d_eff >= p.low_res_distance || p.low_res_distance <= p.high_res_radius) { + return p.max_size; + } + const double t = (d_eff - p.high_res_radius) / (p.low_res_distance - p.high_res_radius); + return p.min_size * std::pow(p.max_size / p.min_size, t); +} + +/// Effective distance for a vertex at `dist_to_known` from ground the robot has driven over: +/// full resolution (high_res_radius) up to known_margin, then the policy's distance grows +/// known_falloff times faster. `dist_to_known - kReach * hypotenuse` is monotone along the vertex +/// hierarchy like the robot's own formula; a constant shift (known_margin), a max with 0 and a +/// positive scale keep that, so the minimum of the two stays crack-free. +/// (The margin is subtracted before the scale: before, it was divided by known_falloff, and with +/// the node's falloff of ~40 a 10 m margin was a 0.25 m strip.) +inline double known_effective_distance(const LodParams & p, double dist_to_known, double hypotenuse) +{ + return p.high_res_radius + + p.known_falloff * std::max(0.0, dist_to_known - p.known_margin - kReach * hypotenuse); +} + +/// The policy for the observed ground: the driven-over one's with observed_margin as its margin +inline LodParams observed_params(const LodParams & p) +{ + LodParams o = p; + if (p.observed_margin >= 0.0) {o.known_margin = p.observed_margin;} + return o; +} + +inline bool should_split_at(const LodParams & p, double error, double leg, double d_eff) +{ + if (leg <= p.min_size) { + return false; + } + return error > tolerance(p, d_eff) || leg > size_cap(p, d_eff); +} + +inline bool should_split( + const LodParams & p, double error, double leg, double dist_to_vertex, double hypotenuse) +{ + return should_split_at(p, error, leg, effective_distance(dist_to_vertex, hypotenuse)); +} + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__LOD_POLICY_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/marker_publisher.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/marker_publisher.hpp new file mode 100644 index 00000000..50b1a30a --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/marker_publisher.hpp @@ -0,0 +1,39 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__MARKER_PUBLISHER_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__MARKER_PUBLISHER_HPP_ + +#include + +#include "visualization_msgs/msg/marker_array.hpp" + +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" + +namespace bigmap_maps_manager +{ + +/// Builds the RViz view of a LOD mesh: +/// - "lod_triangles": TRIANGLE_LIST, per-vertex colour = leg size (green small -> red large) +/// - "lod_wireframe": LINE_LIST of the triangle edges (makes the resolution change obvious) +/// - "lod_high_res_radius": LINE_STRIP circle of high_res_radius around the robot +/// - "lod_traversed_radius": (if outer_radius > 0) the larger circle, in orange +visualization_msgs::msg::MarkerArray make_markers( + const LodMesh & mesh, const LodParams & params, double robot_x, double robot_y, double robot_z, + double max_leg, const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + double outer_radius = 0.0); + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__MARKER_PUBLISHER_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/narrow_passages.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/narrow_passages.hpp new file mode 100644 index 00000000..1640e5fa --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/narrow_passages.hpp @@ -0,0 +1,59 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__NARROW_PASSAGES_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__NARROW_PASSAGES_HPP_ + +#include +#include +#include + +#include "easynav_bigmap_maps_manager/dem_grid.hpp" + +namespace bigmap_maps_manager +{ + +/// Nodes of a height grid the robot can drive on: data present and no 4-neighbour lower by more +/// than the slope allows, i.e. not on the face of a step or a slope (the floor at the foot of a +/// wall is drivable). `valid`: as below. One byte per node, 1 = drivable. +std::vector drivable_nodes( + const DemGrid & dem, const std::vector & valid, double max_slope_deg); + +/// Keeps the ground the robot can get to: the largest piece of drivable nodes (4-connected) and +/// the not drivable nodes that touch it (the faces of walls and slopes, which are the obstacles); +/// every other drivable piece, such as the roofs of buildings or a plateau with no way up, and +/// the steep ground that only touches those, becomes "no data". A goal picked on a roof (the +/// topmost surface under the mouse) would otherwise be a place no plan can reach. +/// Returns the new `valid` (one byte per node, same order as dem.z); `dropped` (if given) is the +/// number of nodes that had data and no longer do. +std::vector keep_main_piece( + const DemGrid & dem, const std::vector & valid, double max_slope_deg, + std::size_t * dropped = nullptr); + +/// Nodes of a height grid that lie in a passage narrower than `width` metres: ground the robot can +/// drive on (slope <= max_slope_deg, data present) whose free width, measured between the nodes +/// that are not drivable (too steep, or no data), is below `width`. The mesher keeps them at the +/// grid's full resolution however far the robot is: coarse NavCels in a narrow passage leave no +/// room for the robot's footprint and the planner cannot find the way through from far away. The +/// grid-engine counterpart of SurfaceLodMesher's keep_narrow_width. +/// +/// `valid`: as LodMesher's (one byte per node, 0 = no data; empty = all data). +/// Returns one byte per node (same order as dem.z), 1 = narrow; empty if width <= 0. +std::vector narrow_passage_nodes( + const DemGrid & dem, const std::vector & valid, + double max_slope_deg, double width); + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__NARROW_PASSAGES_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/navmap_dem.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/navmap_dem.hpp new file mode 100644 index 00000000..5e284593 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/navmap_dem.hpp @@ -0,0 +1,93 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__NAVMAP_DEM_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__NAVMAP_DEM_HPP_ + +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/dem_grid.hpp" +#include "navmap_core/NavMap.hpp" + +namespace bigmap_maps_manager +{ + +/// What the meshers read of a NavMap, as plain arrays: built from a NavMap, or straight from a +/// NavMap file's message (the plugin does that), so that a big map is never built as a NavMap +/// (normals, adjacency, BVH: several times the size of the arrays) only to be read once. +struct SourceMesh +{ + std::vector x, y, z; ///< vertices + std::vector v0, v1, v2; ///< triangles (vertex indices) + std::vector r, g, b; ///< colour per vertex (empty: none) + std::vector traversed; ///< per triangle (empty: the map has no such layer) + std::vector obstacle_hits; ///< per triangle (empty: no such layer) + bool has_traversed_layer{false}; + + std::size_t triangle_count() const {return v0.size();} + bool coloured() const + { + return !r.empty() && r.size() == x.size() && g.size() == x.size() && b.size() == x.size(); + } + /// Throws std::invalid_argument naming what is wrong: arrays of different sizes, a vertex index + /// out of range, a non-finite position (a corrupt file). + void validate() const; + /// The arrays of a NavMap (its `traversed` and `obstacle_hits` layers, if uint8) + static SourceMesh from_navmap(const ::navmap::NavMap & map); +}; + +/// A NavMap of any resolution turned into the regular height grid the LOD mesher works from. +struct NavMapDem +{ + DemGrid dem; ///< centred on the map's (0, 0), no geolocation + /// 1 where the NavMap has surface; the grid is square, the map is not: the rest is no data + std::vector valid; + /// Per grid cell ((side - 1)^2, cell (i, j) between nodes i..i+1 and j..j+1): 1 = a NavCel of + /// the map covers the cell's centre. The holes of the map exactly (no closing: a hole of one + /// cell is an obstacle of one cell), which the LodMesher uses to emit its NavCels. + std::vector cell_valid; + /// DEM nodes (i, j) covered by NavCels of the `traversed` layer, if the map has one + std::vector> traversed_nodes; + bool has_traversed{false}; + std::size_t covered_nodes{0}; +}; + +/// Rasterises the NavMap's NavCels onto a grid of `resolution` metres centred on (0, 0): height by +/// barycentric interpolation (the highest where several NavCels overlap), imagery colour from the +/// vertex colours. Node gaps narrower than two nodes (sampling of very small NavCels) are closed +/// for the heights; the cells (cell_valid) keep every hole. Throws std::invalid_argument for an +/// empty map or a grid beyond `max_side`. +NavMapDem dem_from_navmap(const SourceMesh & map, double resolution, int max_side = 8193); +NavMapDem dem_from_navmap( + const ::navmap::NavMap & map, double resolution, int max_side = 8193); + +/// Georeference of a map, saved next to it: the position of the map's (0, 0, 0). +struct MapOrigin +{ + bool georeferenced{false}; + double lat{0.0}, lon{0.0}, altitude{0.0}; +}; + +/// `.navmap` -> `.yaml` +std::string origin_file_for(const std::string & navmap_path); +bool save_origin(const std::string & navmap_path, const MapOrigin & origin, double resolution); +/// Missing or unreadable file: origin 0, 0, 0 and not georeferenced. +MapOrigin load_origin(const std::string & navmap_path); + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__NAVMAP_DEM_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/obstacle_memory.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/obstacle_memory.hpp new file mode 100644 index 00000000..fa8aca40 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/obstacle_memory.hpp @@ -0,0 +1,72 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__OBSTACLE_MEMORY_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__OBSTACLE_MEMORY_HPP_ + +#include +#include +#include + +#include "easynav_bigmap_maps_manager/dynamic_layer.hpp" + +namespace bigmap_maps_manager +{ + +/// How many times an obstacle has been detected on each spot of the map, whatever the LOD mesh is +/// at the moment (NavCel ids change with every re-mesh, so the count lives on a grid of its own, +/// one cell per `cell` metres and per metre of height, and is read back onto the NavCels). +/// +/// A hit is a detection that went on for `min_time` seconds over the spot (see +/// DynamicLayer::sustained); `max_hits` hits make the obstacle FIXED: it stays for good, whether +/// it is still seen or not. Below that, the count is only the memory of how often it was seen. +class ObstacleMemory +{ +public: + struct Cell + { + float x, y, z; ///< where the first detection was + int hits; ///< 1 .. max_hits + double last; ///< when the last hit was added (s) + }; + + ObstacleMemory(double cell, double min_time, int max_hits) + : cell_(cell), min_time_(min_time), max_hits_(max_hits) {} + + /// Adds a hit to the cell of every detection (one per cell, however many detections it holds). + /// Returns true if any count changed. + bool add(const std::vector & detections, double now); + + /// A cell known from a saved map: `hits` (clamped to 1..max_hits), at least what it had. + /// Its last hit counts as long ago, so a detection can add to it at once. + void restore(double x, double y, double z, int hits); + + double cell_size() const {return cell_;} + int max_hits() const {return max_hits_;} + bool fixed(const Cell & c) const {return c.hits >= max_hits_;} + const std::unordered_map & cells() const {return cells_;} + std::size_t fixed_count() const; + /// Forgets everything (what a recovery would do to let the obstacles go) + void clear() {cells_.clear();} + +private: + std::int64_t key(double x, double y, double z) const; + double cell_, min_time_; + int max_hits_; + std::unordered_map cells_; +}; + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__OBSTACLE_MEMORY_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/pipeline_check.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/pipeline_check.hpp new file mode 100644 index 00000000..1cb05337 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/pipeline_check.hpp @@ -0,0 +1,80 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__PIPELINE_CHECK_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__PIPELINE_CHECK_HPP_ + +#include +#include + +namespace bigmap_maps_manager +{ + +/// Who puts what the sensors see into the `obstacles` layer (parameter obstacles.detector). +enum class ObstacleDetector +{ + /// bigmap's own DynamicLayer (heights relative to the robot, independent of the localization's z) + BigmapDynamic, + /// the ObstacleFilter NavMapFilter plugin of easynav_navmap_maps_manager (it writes `obstacles`) + NavmapFilter, +}; + +/// "bigmap_dynamic" | "navmap_filter", and the names this option had before ("dynamic", "filter"). +/// Throws std::invalid_argument for anything else. +ObstacleDetector parse_detector(const std::string & value); +const char * to_string(ObstacleDetector detector); + +/// A NavMapFilter loaded through the `filters` list: its instance name (the key of its parameters) +/// and the layer it writes ("obstacles": an obstacle filter, "inflated_obstacles": the inflation). +struct LoadedFilter +{ + std::string instance; + std::string layer; +}; + +/// What decides how obstacles reach the planner (see check_pipeline). +struct PipelineSettings +{ + ObstacleDetector detector{ObstacleDetector::BigmapDynamic}; + bool dynamic_map_enabled{false}; ///< dynamic_map.enabled + bool memory_enabled{true}; ///< obstacles.memory.enabled (at start) + bool removal_enabled{true}; ///< obstacles.removal.enabled + std::vector filters; ///< in the order of `filters` +}; + +/// The consequences of a PipelineSettings, for the log, and the combinations that cannot work. +struct PipelineReport +{ + std::vector errors; ///< the node must not start + std::vector warnings; ///< works, but probably not as intended + std::vector notes; ///< what is on and what is off, for the log + bool sensors_processed{false}; ///< bigmap analyses the sensor clouds + bool memory_effective{false}; ///< obstacles.memory does anything + std::string planner_layer; ///< the cost layer A* reads +}; + +/// The dependencies between the options, in one place: +/// - detector navmap_filter needs an obstacle filter in `filters` (else nothing is ever detected); +/// - detector bigmap_dynamic with an obstacle filter listed too: two detectors, one layer; +/// - bigmap's detector works only with obstacles.removal.enabled (it is what puts the detections +/// in `obstacles`; without it they only reach the dynamic map); +/// - bigmap analyses the sensors if dynamic_map.enabled or obstacles.memory.enabled; +/// - obstacles.memory needs the bigmap detector; +/// - the planner reads `inflated_obstacles` if the inflation filter is listed, else `obstacles`; +/// - the obstacle filter must come before the inflation in `filters` (they run in that order). +PipelineReport check_pipeline(const PipelineSettings & s); + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__PIPELINE_CHECK_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/profiler.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/profiler.hpp new file mode 100644 index 00000000..7dbd11c0 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/profiler.hpp @@ -0,0 +1,123 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__PROFILER_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__PROFILER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +namespace bigmap_maps_manager +{ + +/// Accumulates the time of named sections and reports them every `period` seconds (wall clock). +class Profiler +{ +public: + using Clock = std::chrono::steady_clock; + + explicit Profiler(double period_s = 5.0) + : period_(period_s), window_start_(Clock::now()) {} + + void add(const std::string & section, double ms) + { + auto & s = stats_[section]; + s.sum += ms; + s.max = std::max(s.max, ms); + ++s.calls; + } + + double period() const {return period_;} + + /// True once `period` seconds have passed since the last report. + bool due() const + { + return std::chrono::duration(Clock::now() - window_start_).count() >= period_; + } + + /// A table of the window (mean and worst ms per call, calls, and ms spent per second of wall + /// time) sorted by the time each section takes per second; then starts a new window. + std::string report_and_reset(const std::string & title) + { + const double window = std::chrono::duration(Clock::now() - window_start_).count(); + struct Row {std::string name; double mean, max, per_s; std::int64_t calls;}; + std::vector rows; + for (const auto & [name, s] : stats_) { + rows.push_back({name, s.sum / std::max(1L, s.calls), s.max, s.sum / std::max(1e-9, window), + s.calls}); + } + std::sort(rows.begin(), rows.end(), [](const Row & a, const Row & b) { + return a.per_s > b.per_s; + }); + std::string out = title + " (window " + fmt("%.1f", window) + " s)\n"; + out += " section calls mean ms max ms ms per s\n"; + for (const auto & r : rows) { + char line[160]; + std::snprintf( + line, sizeof(line), " %-40s %5ld %9.2f %9.2f %10.1f\n", r.name.c_str(), r.calls, r.mean, + r.max, r.per_s); + out += line; + } + stats_.clear(); + window_start_ = Clock::now(); + return out; + } + +private: + struct Stat + { + double sum{0.0}, max{0.0}; + std::int64_t calls{0}; + }; + static std::string fmt(const char * f, double v) + { + char b[32]; + std::snprintf(b, sizeof(b), f, v); + return b; + } + double period_; + Clock::time_point window_start_; + std::map stats_; +}; + +/// Times its own lifetime into a Profiler; does nothing for a null profiler. +class ProfScope +{ +public: + ProfScope(Profiler * p, const char * name) + : p_(p), name_(name), t0_(Profiler::Clock::now()) {} + ~ProfScope() + { + if (p_) { + p_->add(name_, + std::chrono::duration(Profiler::Clock::now() - t0_).count()); + } + } + ProfScope(const ProfScope &) = delete; + ProfScope & operator=(const ProfScope &) = delete; + +private: + Profiler * p_; + const char * name_; + Profiler::Clock::time_point t0_; +}; + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__PROFILER_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/rtin_error_map.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/rtin_error_map.hpp new file mode 100644 index 00000000..12e64c42 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/rtin_error_map.hpp @@ -0,0 +1,92 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__RTIN_ERROR_MAP_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__RTIN_ERROR_MAP_HPP_ + +#include +#include +#include + +#include "easynav_bigmap_maps_manager/dem_grid.hpp" + +namespace bigmap_maps_manager +{ + +/// RTIN (Right-Triangulated Irregular Network) error map, after Evans et al. / "martini". +/// +/// The DEM is padded (edge replication, on the high-index side) to (2^k + 1)^2 nodes. Every node +/// that is the hypotenuse midpoint of some triangle of the implicit binary triangle tree gets the +/// worst height error incurred by *not* splitting there, propagated monotonically from the +/// descendants (error[parent] >= error[children]). Computed once, O(N log N). +/// +/// Memory: the heights are not copied (they are read from the DEM, clamped to its edge for the +/// padding), and the errors are stored as centimetres in 16 bits, rounded up (so the order +/// error[parent] >= error[children] the crack-free argument needs is kept, and a split is never +/// missed: at most decided for an error up to 1 cm smaller). Errors saturate at 655 m. +/// The DEM must outlive the map (LodMesher owns both). +class RtinErrorMap +{ +public: + /// `pinned`: optional, one byte per DEM node (same order as dem.z): 1 = this node must be kept + /// at full resolution (a narrow passage). Like the error it is propagated up the triangle tree, + /// so a triangle that holds one is flagged and "split(child) => split(parent)" still holds. + /// `hole_nodes` + `hole_error`: optional, one byte per DEM node: 1 = on the border of a hole of + /// the map (an obstacle): its error is at least `hole_error` m, so the triangles around a hole + /// are split, like those over a step of that height, wherever the tolerance is below it, and the + /// hole is not covered by a coarse triangle there. + explicit RtinErrorMap( + const DemGrid & dem, const std::vector & pinned = {}, + const std::vector & hole_nodes = {}, double hole_error = 0.0); + + RtinErrorMap(const RtinErrorMap &) = delete; + RtinErrorMap & operator=(const RtinErrorMap &) = delete; + + int tile_size() const {return tile_size_;} ///< 2^k + int grid_size() const {return tile_size_ + 1;} ///< nodes per axis after padding + int real_side() const {return real_side_;} ///< nodes per axis of the original DEM + + float height(int x, int y) const + { + return dem_->at(std::min(x, real_side_ - 1), std::min(y, real_side_ - 1)); + } + float error(int x, int y) const {return static_cast(errors_[idx(x, y)]) * kErrorStep;} + /// True if the triangle split at this node holds a pinned node + bool pinned(int x, int y) const {return !pinned_.empty() && pinned_[idx(x, y)] != 0;} + + /// Resolution of the stored errors (m) + static constexpr float kErrorStep = 0.01f; + /// Bytes held (the errors, the pinned flags) + std::size_t memory_bytes() const + { + return errors_.capacity() * sizeof(std::uint16_t) + pinned_.capacity(); + } + +private: + std::size_t idx(int x, int y) const + { + return static_cast(y) * grid_size() + x; + } + static std::uint16_t quantize(float e); + + const DemGrid * dem_; + int tile_size_{0}; + int real_side_{0}; + std::vector errors_; + std::vector pinned_; ///< empty: nothing pinned +}; + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__RTIN_ERROR_MAP_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/surface_lod.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/surface_lod.hpp new file mode 100644 index 00000000..430b8bf1 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/surface_lod.hpp @@ -0,0 +1,229 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__SURFACE_LOD_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__SURFACE_LOD_HPP_ + +#include +#include +#include + +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" +#include "easynav_bigmap_maps_manager/lod_policy.hpp" +#include "easynav_bigmap_maps_manager/navmap_dem.hpp" +#include "navmap_core/NavMap.hpp" + +namespace bigmap_maps_manager +{ + +/// Level of detail for a multi-level surface (floors that overlap in x, y, ramps, any NavMap): +/// view-dependent simplification by hierarchical vertex clustering. It works on the NavMap's own +/// triangles, not on a height grid, so several floors over the same (x, y) are not a problem. +/// +/// Built once: level 0 are the (welded) vertices of the map; level k merges the nodes of level +/// k-1 that fall in the same cell of size `min_size * 2^(k-1)` and whose heights span at most +/// `cluster_height` (so two floors are never merged). Each node keeps the mean position and +/// colour of its vertices and the error of replacing them by it. The vertices on the border of +/// the mesh (the sides of a ramp, a hole) only merge with each other, so the border stays where it +/// is instead of shrinking by half a cell: a narrow ramp keeps its width. +/// +/// Per extraction every vertex climbs the tree while the nodes allow it (the policy of LodParams: +/// distance in 3D to the robot, to the ground it has driven over, and the node's error). The +/// result is a cut of the tree, and a triangle is the one its three vertices end in: the mesh is +/// closed wherever the source is (no cracks between different sizes, no seams between floors and +/// ramps), whatever the geometry. +/// +/// Incremental: far from the robot and from the driven-over / observed ground a node's decision +/// does not depend on where the robot is (see `dynamic radius` in the source). That cut and its +/// triangles are computed once; an extraction only climbs again the vertices under the nodes near +/// the robot or that ground, and only remakes the triangles they touch. Its cost is that of the +/// region around the robot, not of the map. extract_reference() is the full algorithm (the same +/// result, for the tests). +class SurfaceLodMesher +{ +public: + struct Config + { + double min_size{1.0}; ///< size of the smallest cluster (leaf) cells, m + double max_size{32.0}; ///< size of the largest ones + double cluster_height{1.2}; ///< a cluster never spans more height than this, m + double vertical_tolerance{1.5}; ///< driven-over ground is the one this close in z, m + /// Height counts this many times in the distance to the robot: the floor above or below is + /// farther than its 3D distance says + double vertical_weight{4.0}; + /// A corridor, ramp or door narrower than this (m) is kept at the full resolution of the map + /// however far the robot is: coarse NavCels in a narrow passage leave no room for the robot's + /// inflation and the planner could not find the way through. 0: off. + double keep_narrow_width{0.0}; + /// Only the largest piece of the map (triangles joined through shared edges) is used: the roof + /// and any other surface the robot cannot get to are dropped. A goal picked on them (the + /// topmost surface under the mouse) could never be reached. + bool drop_unreachable_ground{false}; + /// The clusters on the border of the mesh (holes: obstacles; edges) count as an error of at + /// least this much (m), so they stay fine as far as the tolerance is below it and a hole is + /// not closed by a coarse cluster near the robot. 0: off. + double hole_error{0.0}; + }; + + SurfaceLodMesher(const SourceMesh & map, const Config & config); + SurfaceLodMesher(const ::navmap::NavMap & map, const Config & config); + + SurfaceLodMesher(const SurfaceLodMesher &) = delete; + SurfaceLodMesher & operator=(const SurfaceLodMesher &) = delete; + + /// Marks as driven over the vertices within `radius` (xy) of (x, y) and `vertical_tolerance` + /// of z. Returns how many were new. + int mark(double x, double y, double z, double radius); + /// Forgets the driven-over vertices farther than `radius` (3D) from (x, y, z). + std::size_t forget_outside(double x, double y, double z, double radius); + std::size_t traversed_count() const {return n_known_;} + /// The source map had a `traversed` layer + bool had_traversed_layer() const {return had_traversed_;} + + /// Marks as observed the border vertices (holes, edges: obstacles) within `radius` (xy) of + /// (x, y) and `vertical_tolerance` of z: they stay at full resolution like the driven-over + /// ground (within LodParams::observed_margin), wherever the robot goes: a hole seen from near + /// keeps its shape from far, so the plan does not change with the robot's position and make it + /// come back. Returns how many were new. + int mark_observed(double x, double y, double z, double radius); + /// Forgets the observed vertices farther than `radius` (3D) from (x, y, z). + std::size_t forget_observed_outside(double x, double y, double z, double radius); + std::size_t observed_count() const {return observed_list_.size();} + + LodMesh extract(double robot_x, double robot_y, double robot_z, const LodParams & p) const; + /// The full algorithm (every vertex climbs, every source triangle is mapped): the same mesh as + /// extract(), up to the order of the vertices and triangles. For the tests. + LodMesh extract_reference( + double robot_x, double robot_y, double robot_z, const LodParams & p) const; + + std::size_t vertex_count() const {return levels_[0].x.size();} + std::size_t narrow_vertex_count() const {return n_pinned_;} + std::size_t dropped_triangles() const {return n_dropped_;} + std::size_t source_triangle_count() const {return tri_.size() / 3;} + int level_count() const {return static_cast(levels_.size());} + double source_edge() const {return source_edge_;} + bool coloured() const {return coloured_;} + /// Bytes held by the hierarchy (levels, triangles, indexes, the precomputed far cut) + std::size_t memory_bytes() const; + +private: + struct Level + { + std::vector x, y, z; + std::vector err; ///< error of the node, m (empty for level 0) + std::vector r, g, b; ///< mean colour (empty if the map has none) + std::vector flags; ///< kBoundary | kPinned + std::vector parent; ///< node of the next level + std::vector child_start, child; ///< nodes of the previous level (CSR) + double size{0.0}; + float max_err{0.0f}; ///< the largest err of the level + }; + static constexpr std::uint8_t kBoundary = 1, kPinned = 2; + + /// Items (vertices or nodes) by square cell of `cell` m, as a CSR over the sorted cell keys + struct CellIndex + { + double cell{2.0}; + std::vector keys; + std::vector start, items; + void build(double cell_size, const std::vector & x, const std::vector & y); + /// Calls f(item) for the items of the cells that meet the disc (a superset of the disc) + template + void visit_disc(double x, double y, double radius, F && f) const; + template + void visit_cell(std::int64_t key, F && f) const; + std::size_t memory_bytes() const; + }; + + struct KdNode + { + float x, y, z; + bool by_x; + }; + struct Query; + + void init(const SourceMesh & map); + void build_levels(); + void pin_narrow_passages(); + void build_far_cut(const LodParams & p) const; + bool far_cut_matches(const LodParams & p) const; + void next_epoch() const; + int level_of(std::uint32_t global) const; + std::uint32_t static_leaf(std::uint32_t v) const; + std::uint32_t climb(std::uint32_t v, const Query & q) const; + + bool static_allowed(int k, std::uint32_t c) const; + bool allowed_now(int k, std::uint32_t c, const Query & q) const; + bool allowed_memo(int k, std::uint32_t c, const Query & q) const; + void build_kd() const; + static void kd_build(std::vector & kd, std::size_t lo, std::size_t hi); + void kd_nearest( + const std::vector & kd, std::size_t lo, std::size_t hi, double x, double y, double z, + double & best) const; + double near_distance( + const std::vector & kd, double x, double y, double z, double reach) const; + + Config cfg_; + bool coloured_{false}; + double source_edge_{0.0}; ///< median edge length of the source, m + std::size_t n_pinned_{0}; + std::size_t n_dropped_{0}; + bool had_traversed_{false}; + std::vector levels_; + std::vector offset_; // global id of node (k, c) = offset_[k] + c + std::vector tri_; // welded vertex ids, 3 per source triangle + std::vector tri_slope_; // per source triangle: degrees x 100, rounded up + std::vector vtx_tri_start_, vtx_tri_; // the source triangles of each vertex + std::vector known_; // per vertex: driven over + std::vector known_list_; + std::atomic n_known_{0}; // read by the save services from another thread + std::vector observed_; // per vertex: an obstacle border seen near + std::vector observed_list_; + CellIndex grid_; // vertices by 2 m cell + std::vector node_grid_; // per level (k >= 1): nodes by cell + static constexpr double kGridCell = 2.0; + + // The driven-over and the observed vertices as 2-D trees, rebuilt when they change + mutable std::vector kd_known_, kd_observed_; + mutable bool kd_dirty_{true}; + + // The far cut: the one of a robot infinitely far, with no driven-over or observed ground (it + // depends on the policy's min_size, max_size, base_error and error_growth only, and is rebuilt + // when they change). Its triangles (global node ids, in the order of the first source triangle + // that ends in each), the steepest source slope, the largest node size, their source triangles + // (CSR, in source order) and, per source triangle, its far triangle (kNone: degenerate) + mutable bool far_built_{false}; + mutable double far_min_{0.0}, far_max_{0.0}, far_base_{0.0}, far_growth_{0.0}; + mutable std::vector far_tri_; + mutable std::vector far_slope_; + mutable std::vector far_size_; + mutable std::vector far_src_start_, far_src_, far_of_src_; + static constexpr std::uint32_t kNone = 0xffffffffu; + + // Scratch of extract() (not thread-safe: one extraction at a time), reset by stamps + mutable std::uint32_t epoch_{0}; + mutable std::vector node_stamp_; // per global node: decision memoised + mutable std::vector node_memo_; // the decision + mutable std::vector walk_stamp_; // per global node: subtree collected + mutable std::vector slot_stamp_, slot_; // per global node: output vertex + mutable std::vector known_stamp_; // per global node: holds a driven-over vertex + mutable std::vector vtx_stamp_; // per vertex: climbed again + mutable std::vector cur_node_; // its node (global id), if climbed again + mutable std::vector far_stamp_; // per far triangle: remade + mutable std::vector src_stamp_; // per source triangle: remade +}; + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__SURFACE_LOD_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/update_watchdog.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/update_watchdog.hpp new file mode 100644 index 00000000..6bdc7527 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/update_watchdog.hpp @@ -0,0 +1,91 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__UPDATE_WATCHDOG_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__UPDATE_WATCHDOG_HPP_ + +#include + +namespace bigmap_maps_manager +{ + +/// Tells, on the wall clock, when update() has stopped being called. +/// +/// EasyNav runs a maps manager when its ROS clock says it is due. If that clock never moves (a +/// node with use_sim_time and no /clock reaching it) update() is never called, and every log +/// throttled on the ROS clock is silent too: nothing says why there is no map. ran() is called by +/// update() and check() by a wall timer, from another thread: the time of the last run is atomic, +/// the rest is only touched by check(). +class UpdateWatchdog +{ +public: + enum class Event {None, Stalled, Resumed}; + + /// `grace_s`: how long update() may not run before it is a stall (also from start()); + /// `repeat_s`: how often a stall that goes on is told again. + explicit UpdateWatchdog(double grace_s = 10.0, double repeat_s = 30.0) + : grace_(grace_s), repeat_(repeat_s) {} + + /// From now on update() is expected (seconds of a monotonic clock) + void start(double now_s) + { + last_ran_.store(now_s); + started_ = true; + stalled_ = false; + } + + /// update() ran + void ran(double now_s) {last_ran_.store(now_s);} + + /// What to tell now; stalled_s() is how long update() has not run. + Event check(double now_s) + { + if (!started_) {return Event::None;} + const double last = last_ran_.load(); + stalled_s_ = now_s - last; + if (stalled_) { + if (last > stalled_since_) { + stalled_ = false; + return Event::Resumed; + } + if (now_s - last_told_ >= repeat_) { + last_told_ = now_s; + return Event::Stalled; + } + return Event::None; + } + if (stalled_s_ > grace_) { + stalled_ = true; + stalled_since_ = last; + last_told_ = now_s; + return Event::Stalled; + } + return Event::None; + } + + double stalled_s() const {return stalled_s_;} + +private: + double grace_, repeat_; + std::atomic last_ran_{0.0}; + bool started_{false}; + bool stalled_{false}; + double stalled_since_{0.0}; // the last run before the stall + double last_told_{0.0}; + double stalled_s_{0.0}; +}; + +} // namespace bigmap_maps_manager + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__UPDATE_WATCHDOG_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/package.xml b/maps_managers/easynav_bigmap_maps_manager/package.xml new file mode 100644 index 00000000..63a1c10e --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/package.xml @@ -0,0 +1,38 @@ + + + + easynav_bigmap_maps_manager + 0.5.1 + Easy Navigation: BigMap maps manager. A map for areas too big to plan on at full resolution: a NavMap whose triangles are fine around the robot and coarse far away (dynamic level of detail of a DEM or of a NavMap with several floors). + José Miguel Guerrero + Apache-2.0 + José Miguel Guerrero + + ament_cmake + + rclcpp + geometry_msgs + visualization_msgs + tf2 + nav_msgs + std_srvs + rclcpp_lifecycle + pluginlib + easynav_common + easynav_core + easynav_sensors + easynav_navmap_maps_manager + navmap_core + navmap_ros + navmap_ros_interfaces + std_msgs + eigen + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/maps_managers/easynav_bigmap_maps_manager/pixi.toml b/maps_managers/easynav_bigmap_maps_manager/pixi.toml new file mode 100644 index 00000000..54ac8daf --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/pixi.toml @@ -0,0 +1,7 @@ +# Conda package built by pixi (pixi-build-ros): name, version and deps come from package.xml +[package] +publish = true + +[package.build.backend] +name = "pixi-build-ros" +workspace = true diff --git a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp new file mode 100644 index 00000000..cd91f1ba --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -0,0 +1,1825 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/BigMapMapsManager.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/marker_publisher.hpp" +#include "easynav_bigmap_maps_manager/narrow_passages.hpp" +#include "easynav_bigmap_maps_manager/navmap_dem.hpp" +#include "easynav_bigmap_maps_manager/pipeline_check.hpp" +#include "easynav_common/PackageShare.hpp" +#include "easynav_common/Parameters.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_bigmap_maps_manager/dem_grid.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "navmap_core/Costs.hpp" +#include "navmap_ros/conversions.hpp" +#include "navmap_ros/navmap_io.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "std_msgs/msg/header.hpp" + +namespace easynav +{ +namespace bigmap +{ + +namespace +{ +// "~" and "~/..." are the user's home; "~user/..." is left as it is (not supported) +std::string expand(std::string p) +{ + if (p == "~" || p.rfind("~/", 0) == 0) { + const char * home = std::getenv("HOME"); + p = std::string(home ? home : "") + p.substr(1); + } + return p; +} +// key of a cell of a 2D grid (indices shifted as unsigned: negative ones are fine) +std::int64_t grid_key(std::int64_t i, std::int64_t j) +{ + return static_cast(static_cast(i) << 32) ^ (j & 0xffffffffLL); +} +// The arrays of a NavMap file's message, moved out of it: a big map is never built as a NavMap +// (normals, adjacency, BVH: several times the arrays) only to be read once. Its `traversed` and +// `obstacle_hits` layers, whatever their type (a value > 0 counts). +bigmap_maps_manager::SourceMesh source_from_msg(navmap_ros_interfaces::msg::NavMap && msg) +{ + bigmap_maps_manager::SourceMesh m; + m.x = std::move(msg.positions_x); + m.y = std::move(msg.positions_y); + m.z = std::move(msg.positions_z); + m.v0 = std::move(msg.navcels_v0); + m.v1 = std::move(msg.navcels_v1); + m.v2 = std::move(msg.navcels_v2); + const std::size_t nv = m.x.size(), nt = m.v0.size(); + if (msg.has_vertex_rgba && msg.colors_r.size() == nv && msg.colors_g.size() == nv && + msg.colors_b.size() == nv) + { + m.r = std::move(msg.colors_r); + m.g = std::move(msg.colors_g); + m.b = std::move(msg.colors_b); + } + for (auto & l : msg.layers) { + if ((l.name != "traversed" && l.name != "obstacle_hits") || !l.indices.empty()) {continue;} + auto & out = l.name == "traversed" ? m.traversed : m.obstacle_hits; + const auto take = [&out, nt](const auto & data) { + if (data.size() != nt) {return;} + out.resize(nt); + for (std::size_t t = 0; t < nt; ++t) { + const double v = static_cast(data[t]); + out[t] = static_cast(v > 0.0 ? std::min(v, 255.0) : 0.0); + } + }; + switch (l.type) { + case navmap_ros_interfaces::msg::NavMapLayer::U8: + if (l.data_u8.size() == nt) {out = std::move(l.data_u8);} + break; + case navmap_ros_interfaces::msg::NavMapLayer::F32: take(l.data_f32); break; + case navmap_ros_interfaces::msg::NavMapLayer::F64: take(l.data_f64); break; + case navmap_ros_interfaces::msg::NavMapLayer::I16: take(l.data_i16); break; + default: break; + } + if (l.name == "traversed") {m.has_traversed_layer = true;} + } + msg = navmap_ros_interfaces::msg::NavMap(); + return m; +} +// seconds of the monotonic wall clock (the watchdog of update() must not use the ROS clock) +double wall_seconds() +{ + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()).count(); +} +} // namespace + +void BigMapMapsManager::create_services(const std::string & base) +{ + auto node = get_node(); + // The service only queues the request: EasyNav serves it from its non-real-time executor while + // update() runs in the real-time one, so the file is written by update() (the thread that owns + // the mesh) in its next cycle, and the result is logged. What the callback reads here (whether + // anything was traversed) is an atomic count + auto request_save = [this](bool traversed_only, std_srvs::srv::Trigger::Response & res) { + if (traversed_only && !has_known()) { + res.success = false; + res.message = "the robot has not driven over any ground yet"; + return; + } + auto req = std::make_shared(); + req->traversed_only = traversed_only; + char stamp[32]; + const std::time_t tt = std::time(nullptr); + std::tm local{}; + localtime_r(&tt, &local); + std::strftime(stamp, sizeof(stamp), "%Y%m%d_%H%M%S", &local); + const std::string stem = save_directory_ + "/map_" + + (traversed_only ? "traversed_" : "all_") + + stamp; + req->path = stem + ".navmap"; + // a second save within the same second must not overwrite the first one + for (int n = 2; std::filesystem::exists(req->path) && n < 1000; ++n) { + req->path = stem + "_" + std::to_string(n) + ".navmap"; + } + std::lock_guard lock(save_mutex_); + if (pending_save_) { + res.success = false; + res.message = "another save is in progress"; + return; + } + pending_save_ = req; + res.success = true; + res.message = "writing " + req->path + " (see the maps manager log)"; + }; + save_all_srv_ = node->create_service( + base + "/save_map", + [request_save]( + const std::shared_ptr, + std::shared_ptr res) {request_save(false, *res);}); + save_traversed_srv_ = node->create_service( + base + "/save_traversed", + [request_save]( + const std::shared_ptr, + std::shared_ptr res) {request_save(true, *res);}); +// For a recovery: forget every obstacle the memory holds, fixed ones included. Queued like the +// saves (update() does it in its next cycle); without memory there is nothing to forget + clear_obstacles_srv_ = node->create_service( + base + "/clear_obstacles", + [this]( + const std::shared_ptr, + std::shared_ptr res) { + res->success = memory_ != nullptr; + if (memory_) { + clear_obstacles_requested_ = true; + res->message = "the obstacle memory is cleared in the next cycle"; + } else { + res->message = "this plugin has no obstacle memory (obstacles.detector: navmap_filter)"; + } + }); +} + +void BigMapMapsManager::on_initialize() +{ + auto node = get_node(); + const std::string name = get_plugin_name(); + auto param = [&](const std::string & key, auto def) { + easynav::declare_parameter_if_absent(*node, name + "." + key, def); + node->get_parameter(name + "." + key, def); + return def; + }; + // New parameter names, with the previous ones still accepted (the first one set wins) + const auto & overrides = node->get_node_parameters_interface()->get_parameter_overrides(); + std::vector deprecated; + auto param_any = [&](std::initializer_list keys, auto def) { + const char * chosen = *keys.begin(); + for (const char * k : keys) { + if (overrides.count(name + "." + k)) {chosen = k; break;} + } + if (chosen != *keys.begin()) { + deprecated.push_back(std::string(chosen) + " -> " + *keys.begin()); + } + return param(chosen, def); + }; + + const std::string dem_file = expand(param_any({"terrain.dem_file", "dem_file"}, std::string())); + const double wait_s = param_any({"terrain.dem_wait_timeout", "dem_wait_timeout"}, 60.0); + params_.high_res_radius = param_any({"lengths.radii.full_res", "lod_settings.high_res_radius"}, + 10.0); + params_.min_size = param_any({"lengths.cell_size.min", "lod_settings.min_triangle_size"}, 1.0); + params_.max_size = param_any({"lengths.cell_size.max", "lod_settings.max_triangle_size"}, 32.0); + params_.base_error = param_any({"lod.base_error", "lod_settings.base_error"}, 0.02); + params_.error_growth = param_any({"lod.error_growth", "lod_settings.error_growth"}, 0.05); + params_.low_res_distance = param_any({"lengths.radii.low_res_start", + "lod_settings.low_res_distance"}, 200.0); + hysteresis_ = param_any({"lengths.remesh_step", "lod_settings.hysteresis"}, 2.0); + profile_enabled_ = param_any({"output.profile.enabled", "profile.enabled"}, false); + profiler_ = bigmap_maps_manager::Profiler(param_any({"output.profile.period", "profile.period"}, + 5.0)); + save_directory_ = expand(param_any({"output.save_directory", "save.directory"}, + std::string("~/.ros/bigmap/saved"))); + trail_enabled_ = param_any({"traversed.enabled", "traversed_ground.enabled"}, true); + trail_mark_radius_ = param_any({"lengths.radii.traversed_mark", "traversed_ground.mark_radius"}, + 1.0); + { + // circles: inside the small circle (high_res_radius) everything is at full resolution; + // between it and the larger one (keep_radius) only the ground driven over; + // beyond: low, and what was driven over there is forgotten until it is driven + // over again. + // persistent: inside the small circle everything; outside it only the ground driven over, for + // good, wherever it is. + const std::string mode = param_any({"traversed.mode", "traversed_ground.mode"}, + std::string("circles")); + if (mode != "circles" && mode != "persistent") { + throw std::invalid_argument("[" + name + "] traversed.mode: circles or persistent"); + } + trail_persistent_ = mode == "persistent"; + trail_keep_radius_ = param_any({"lengths.radii.traversed_keep", "traversed_ground.keep_radius"}, + 40.0); + } + { + // radius that stays at full resolution around the ground driven over (0: the same as the + // robot's own, high_res_radius), and the distance beyond it over which the resolution fades + const double margin = param_any({"lengths.radii.traversed_full_res", "traversed_ground.radius"}, + 0.0); + params_.known_margin = margin > 0.0 ? margin : params_.high_res_radius; + const double fade = std::max(0.5, + param_any({"lengths.traversed_fade", "traversed_ground.falloff_distance"}, 5.0)); + params_.known_falloff = std::max( + 1.0, (params_.low_res_distance - params_.high_res_radius) / fade); + } + max_slope_deg_ = param_any({"terrain.max_slope_deg", "max_slope_deg"}, 35.0); + // the holes of the map (its obstacles): their border counts as this height error (m) + const double hole_error = param("lod.hole_error", 0.0); + // obstacle borders seen from near stay at full resolution (within observed_full_res of them) + observed_enabled_ = param("lod.observed.enabled", true); + observed_mark_radius_ = param("lengths.radii.observed_mark", params_.high_res_radius); + params_.observed_margin = param("lengths.radii.observed_full_res", 1.0); + recovery_enabled_ = param("recovery.enabled", true); + recovery_cycles_ = static_cast(param("recovery.no_path_cycles", int64_t{3})); + recovery_max_factor_ = param("recovery.max_full_res_factor", 4.0); + async_remesh_ = param("lod.async_remesh", false); + if (hole_error < 0.0 || observed_mark_radius_ < 0.0 || params_.observed_margin < 0.0 || + recovery_cycles_ < 1 || recovery_max_factor_ < 1.0) + { + throw std::invalid_argument( + "[" + name + "] lod.hole_error >= 0, lengths.radii.observed_mark >= 0, " + "lengths.radii.observed_full_res >= 0, recovery.no_path_cycles >= 1 and " + "recovery.max_full_res_factor >= 1 are required"); + } + publish_markers_ = param_any({"output.publish_markers", "publish_markers"}, true); + dynamic_enabled_ = param("dynamic_map.enabled", true); + { + // Only what the sensors see inside the small circle (lengths.radii.full_res, the blue + // one) is used. max_range can make it smaller; 0 = the whole circle. + const double requested = param_any({"lengths.radii.sensor_range", "dynamic_map.max_range"}, + 0.0); + dyn_.max_range = requested > 0.0 ? + std::min(requested, params_.high_res_radius) : params_.high_res_radius; + } + dyn_.min_height = param("dynamic_map.min_height", 0.3); + dyn_.min_vertical_extent = param("dynamic_map.min_vertical_extent", 0.2); + dyn_.confirm_time = param("dynamic_map.confirm_time", 0.3); + dyn_.max_height = param("dynamic_map.max_height", 3.0); + dyn_.forget_time = param("dynamic_map.forget_time", 5.0); + dyn_.cell = param("dynamic_map.cell", 0.25); + dyn_.voxel = param("dynamic_map.voxel", 0.1); + dyn_.ground_fit = param("dynamic_map.ground_fit.enabled", true); + dyn_.window_length = param("dynamic_map.ground_fit.window_length", 4.0); + dyn_.window_width = param("dynamic_map.ground_fit.window_width", 3.0); + dyn_.window_z_min = param("dynamic_map.ground_fit.z_min", -1.0); + dyn_.window_z_max = param("dynamic_map.ground_fit.z_max", 2.5); + dyn_.ground_tolerance = param("dynamic_map.ground_fit.tolerance", 0.08); + dyn_.max_ground_tilt_deg = param("dynamic_map.ground_fit.max_tilt_deg", 40.0); + dyn_.ground_min_cells = static_cast(param("dynamic_map.ground_fit.min_cells", 12)); + if (dyn_.cell <= 0.0 || dyn_.max_range <= 0.0 || dyn_.forget_time < 0.0 || + dyn_.voxel < 0.0 || dyn_.ground_min_cells < 3) + { + // (a plane needs 3 cells: fewer would make the RANSAC draw from an empty set) + throw std::invalid_argument( + "[" + name + "] invalid dynamic_map parameters: cell > 0, forget_time >= 0, " + "voxel >= 0 and ground_fit.min_cells >= 3 are required"); + } + view_max_z_ = param_any({"output.view_max_z", "view_max_z"}, 1e9); + tint_enabled_ = param_any({"obstacles.tint.enabled", "obstacle_tint.enabled"}, true); + tint_strength_ = static_cast(param_any({"obstacles.tint.strength", + "obstacle_tint.strength"}, 0.65)); + { + const auto rgb = param_any({"obstacles.tint.color", "obstacle_tint.color"}, + std::vector{255, 40, 40}); + if (rgb.size() != 3) { + throw std::invalid_argument("[" + name + "] obstacles.tint.color needs 3 values"); + } + for (std::size_t i = 0; i < 3; ++i) { + tint_rgb_[i] = static_cast(std::clamp(rgb[i], 0, 255)); + } + tint_strength_ = std::clamp(tint_strength_, 0.0f, 1.0f); + } + // obstacles.detector (before: obstacles.source / obstacle_source, values dynamic | filter) + const auto detector = bigmap_maps_manager::parse_detector( + param_any({"obstacles.detector", "obstacles.source", "obstacle_source"}, + std::string("bigmap_dynamic"))); + obstacles_from_filter_ = detector == bigmap_maps_manager::ObstacleDetector::NavmapFilter; + removal_enabled_ = param_any({"obstacles.removal.enabled", "obstacle_removal.enabled"}, true); + removal_radius_ = param_any({"lengths.radii.obstacle_clearance", "obstacle_removal.radius"}, 1.0); + if (removal_radius_ < 0.0) { + throw std::invalid_argument("[" + name + "] lengths.radii.obstacle_clearance must be >= 0"); + } + bool memory_enabled_at_start = false; // for the pipeline check below + { + // Memory of obstacles: a detection that lasts memory.min_time s over a NavCel is a hit; the + // count is the layer `obstacle_hits`; memory.max_hits hits fix the obstacle for good + const bool enabled = param_any({"obstacles.memory.enabled"}, true); + memory_enabled_at_start = enabled; + memory_min_time_ = param_any({"obstacles.memory.min_time"}, 5.0); + memory_max_hits_ = static_cast(param_any({"obstacles.memory.max_hits"}, int64_t{3})); + if (memory_min_time_ <= 0.0 || memory_max_hits_ < 1 || memory_max_hits_ > 255) { + throw std::invalid_argument( + "[" + name + + "] obstacles.memory: min_time > 0 and 1 <= max_hits <= 255 are required"); + } + } + + if (!deprecated.empty()) { + std::string list; + for (const auto & d : deprecated) { + list += (list.empty() ? "" : ", ") + d; + } + RCLCPP_WARN( + node->get_logger(), "[%s] old parameter names still accepted, please rename: %s", + name.c_str(), list.c_str()); + } + if (params_.min_size <= 0.0 || params_.max_size < params_.min_size || + params_.high_res_radius < 0.0 || params_.low_res_distance < params_.high_res_radius || + hysteresis_ <= 0.0) + { + throw std::invalid_argument("[" + name + + "] invalid lengths: cell_size.min > 0, cell_size.max >= cell_size.min, " + "radii.full_res >= 0, radii.low_res_start >= radii.full_res and remesh_step > 0 " + "are required"); + } + base_high_res_ = params_.high_res_radius; + base_low_res_ = params_.low_res_distance; + if (profile_enabled_ && profiler_.period() <= 0.0) { + throw std::invalid_argument("[" + name + "] output.profile.period must be > 0"); + } + if (trail_enabled_ && !trail_persistent_ && trail_keep_radius_ < params_.high_res_radius) { + throw std::invalid_argument( + "[" + name + "] lengths.radii.traversed_keep must be >= lengths.radii.full_res"); + } + std::string initial_navmap = expand(param_any({"terrain.initial_navmap", "initial_navmap"}, + std::string())); + { + // terrain.package: a relative initial_navmap is in that package's share directory (as the + // Bonxai map's `package` + `bonxai_path_file`), so the parameter file says it all + const auto package = param_any({"terrain.package"}, std::string()); + if (!package.empty() && !initial_navmap.empty() && + !std::filesystem::path(initial_navmap).is_absolute()) + { + try { + initial_navmap = (easynav::get_package_share_path(package) / initial_navmap).string(); + } catch (const std::exception & e) { + throw std::runtime_error("[" + name + "] terrain.package '" + package + "': " + e.what()); + } + } + } + const bool multilevel = param("multilevel.enabled", false); + bigmap_maps_manager::SurfaceLodMesher::Config surface_cfg; + surface_cfg.cluster_height = param("multilevel.cluster_height", 1.2); + surface_cfg.vertical_tolerance = param("multilevel.vertical_tolerance", 1.5); + surface_cfg.vertical_weight = param("multilevel.vertical_weight", 4.0); + const double keep_narrow = param_any( + {"terrain.keep_narrow_width", "multilevel.keep_narrow_width"}, 0.0); + surface_cfg.keep_narrow_width = keep_narrow; + const bool drop_unreachable = param("terrain.drop_unreachable_ground", false); + surface_cfg.drop_unreachable_ground = drop_unreachable; + surface_cfg.hole_error = hole_error; + if (multilevel && initial_navmap.empty()) { + throw std::invalid_argument("[" + name + "] multilevel.enabled needs terrain.initial_navmap"); + } + std::optional dem; + std::vector valid; + std::vector cell_valid; // the holes of a NavMap, exactly (see NavMapDem) + std::vector> restored; + std::vector> saved_hits; // x, y, z, hits of a saved map's obstacle_hits + if (!initial_navmap.empty()) { + // Any NavMap, whatever its resolution: it is turned into the height grid the LOD works from + // (centred on its own (0, 0)) and the robot's movement refines it from there + // read as the message's arrays (see source_from_msg): never as a NavMap + bigmap_maps_manager::SourceMesh loaded; + { + navmap_ros_interfaces::msg::NavMap msg; + std::error_code ec; + if (!navmap_ros::io::load_msg_from_file(initial_navmap, msg, &ec)) { + throw std::runtime_error( + "[" + name + "] cannot load initial_navmap '" + initial_navmap + "' (" + + ec.message() + ")"); + } + loaded = source_from_msg(std::move(msg)); + } + // a corrupt file (a NaN vertex, an index out of range) would reach every mesh, or an index + // cast: refused here, by name + try { + loaded.validate(); + } catch (const std::invalid_argument & e) { + throw std::runtime_error( + "[" + name + "] initial_navmap '" + initial_navmap + "': " + e.what()); + } + if (loaded.triangle_count() == 0) { + throw std::runtime_error("[" + name + "] initial_navmap '" + initial_navmap + + "' has no NavCels"); + } + origin_ = bigmap_maps_manager::load_origin(initial_navmap); + if (!loaded.obstacle_hits.empty()) { + // what a saved map counted (steep ground is no obstacle of the memory: it is steep anyway) + const float min_nz = static_cast(std::cos(max_slope_deg_ * M_PI / 180.0)); + const auto & X = loaded.x; + const auto & Y = loaded.y; + const auto & Z = loaded.z; + for (std::size_t c = 0; c < loaded.triangle_count(); ++c) { + const auto hits = loaded.obstacle_hits[c]; + if (hits == 0) {continue;} + const std::array v{loaded.v0[c], loaded.v1[c], loaded.v2[c]}; + const double ux = X[v[1]] - X[v[0]], uy = Y[v[1]] - Y[v[0]], uz = Z[v[1]] - Z[v[0]], + wx = X[v[2]] - X[v[0]], wy = Y[v[2]] - Y[v[0]], wz = Z[v[2]] - Z[v[0]]; + const double nx = uy * wz - uz * wy, ny = uz * wx - ux * wz, nz = ux * wy - uy * wx; + const double len = std::sqrt(nx * nx + ny * ny + nz * nz); + if (len < 1e-12 || std::abs(nz) / len < min_nz) {continue;} + saved_hits.push_back({(X[v[0]] + X[v[1]] + X[v[2]]) / 3.0f, + (Y[v[0]] + Y[v[1]] + Y[v[2]]) / 3.0f, (Z[v[0]] + Z[v[1]] + Z[v[2]]) / 3.0f, + static_cast(hits)}); + } + } + if (multilevel) { + surface_cfg.min_size = params_.min_size; + surface_cfg.max_size = params_.max_size; + surface_ = std::make_unique(loaded, surface_cfg); + if (surface_->dropped_triangles() > 0) { + RCLCPP_INFO( + node->get_logger(), + "[%s] %zu NavCels not joined to the largest piece of the map (the roof...) are dropped", + name.c_str(), surface_->dropped_triangles()); + } + RCLCPP_INFO( + node->get_logger(), + "[%s] initial NavMap %s (multi-level): %zu NavCels, %zu welded vertices (%zu in narrow " + "passages, kept at full resolution), %d levels of detail, %.1f MB; %s; %s", name.c_str(), + initial_navmap.c_str(), loaded.triangle_count(), surface_->vertex_count(), + surface_->narrow_vertex_count(), surface_->level_count(), + surface_->memory_bytes() / 1e6, + origin_.georeferenced ? "georeferenced (origin file found)" : + "no geolocation (origin 0, 0, 0)", + surface_->had_traversed_layer() ? "with a traversed layer" : "no traversed layer"); + } else { + const double res_param = param_any({"terrain.initial_navmap_resolution", + "initial_navmap_resolution"}, 0.0); + const double res = res_param > 0.0 ? res_param : params_.min_size; + auto nd = bigmap_maps_manager::dem_from_navmap(loaded, res); + nd.dem.origin_lat = origin_.lat; + nd.dem.origin_lon = origin_.lon; + nd.dem.reference_altitude = origin_.altitude; + dem = std::move(nd.dem); + valid = std::move(nd.valid); + cell_valid = std::move(nd.cell_valid); + restored = std::move(nd.traversed_nodes); + RCLCPP_INFO( + node->get_logger(), + "[%s] initial NavMap %s: %zu NavCels -> %dx%d grid @ %.2f m; %s; %s", name.c_str(), + initial_navmap.c_str(), loaded.triangle_count(), dem->side, dem->side, res, + origin_.georeferenced ? "georeferenced (origin file found)" : + "no geolocation (origin 0, 0, 0)", + nd.has_traversed ? "with a traversed layer" : "no traversed layer"); + } + } else { + if (dem_file.empty()) { + throw std::invalid_argument( + "[" + name + "] parameter terrain.dem_file (or terrain.initial_navmap) is required"); + } + // The generator (navmap_gis_tool --dem-out) writes the DEM atomically; wait for it if needed + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::duration(wait_s); + while (!(dem = bigmap_maps_manager::DemGrid::load(dem_file))) { + if (std::chrono::steady_clock::now() > deadline) { + throw std::runtime_error( + "[" + name + "] cannot load DEM '" + dem_file + + "' (run navmap_gis_tool --dem-out first)"); + } + RCLCPP_INFO_THROTTLE( + node->get_logger(), *node->get_clock(), 5000, "[%s] waiting for DEM '%s'...", + name.c_str(), dem_file.c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + origin_ = {true, dem->origin_lat, dem->origin_lon, dem->reference_altitude}; + if (!dem->cell_valid.empty()) { + // a DEM with holes (buildings, walls: navmap_gis_tool --dem-holes-mesh): its cells are the + // ground the robot can be on, as a NavMap's are (exact holes, hole_error, observed borders); + // a node is valid if one of its cells has ground + cell_valid = std::move(dem->cell_valid); + dem->cell_valid.clear(); + const int side = dem->side, nc = side - 1; + valid.assign(static_cast(side) * side, 0); + for (int j = 0; j < nc; ++j) { + for (int i = 0; i < nc; ++i) { + if (!cell_valid[static_cast(j) * nc + i]) {continue;} + for (const auto & [di, dj] : {std::pair{0, 0}, std::pair{1, 0}, std::pair{0, 1}, + std::pair{1, 1}}) + { + valid[static_cast(j + dj) * side + i + di] = 1; + } + } + } + RCLCPP_INFO( + node->get_logger(), "[%s] the DEM has holes: %zu of %zu cells without ground", + name.c_str(), + static_cast(std::count(cell_valid.begin(), cell_valid.end(), 0)), + cell_valid.size()); + } + } + if (surface_) { + if (surface_->traversed_count() > 0) { + RCLCPP_INFO( + node->get_logger(), "[%s] restored %zu traversed vertices", name.c_str(), + surface_->traversed_count()); + } + } else { + params_.min_size = std::max(params_.min_size, dem->resolution); // cannot beat the data + if (drop_unreachable) { + std::size_t dropped = 0; + auto kept = bigmap_maps_manager::keep_main_piece(*dem, valid, max_slope_deg_, &dropped); + if (!cell_valid.empty()) { + // the cells with a corner on a dropped piece go with it + const int side = dem->side, nc = side - 1; + const auto gone = [&](int i, int j) { + const auto k = static_cast(j) * side + i; + return valid[k] && !kept[k]; + }; + for (int j = 0; j < nc; ++j) { + for (int i = 0; i < nc; ++i) { + if (gone(i, j) || gone(i + 1, j) || gone(i, j + 1) || gone(i + 1, j + 1)) { + cell_valid[static_cast(j) * nc + i] = 0; + } + } + } + } + valid = std::move(kept); + RCLCPP_INFO( + node->get_logger(), "[%s] %zu nodes of the grid are on ground the robot cannot get to " + "(roofs, plateaus with no way up) and are dropped", name.c_str(), dropped); + } + const auto pinned = bigmap_maps_manager::narrow_passage_nodes( + *dem, valid, max_slope_deg_, keep_narrow); + const auto n_pinned = static_cast(std::count(pinned.begin(), pinned.end(), 1)); + if (keep_narrow > 0.0) { + RCLCPP_INFO( + node->get_logger(), "[%s] %zu of %zu nodes in passages narrower than %.1f m, kept at full " + "resolution", name.c_str(), n_pinned, dem->z.size(), keep_narrow); + } + const int side = dem->side; + const double resolution = dem->resolution; + mesher_ = std::make_unique( + std::move(*dem), std::move(valid), pinned, std::move(cell_valid), hole_error); + dem.reset(); + known_ = std::make_unique(side, resolution); + if (observed_enabled_ && !mesher_->cell_valid().empty()) { + observed_ = std::make_unique(side, resolution); + } + RCLCPP_INFO( + node->get_logger(), "[%s] grid mesher: %.1f MB (height grid, error map, masks)", + name.c_str(), mesher_->memory_bytes() / 1e6); + for (const auto & [i, j] : restored) { + known_->mark_node(i, j); + } + if (!restored.empty()) { + RCLCPP_INFO( + node->get_logger(), "[%s] restored %zu traversed nodes", name.c_str(), known_->count()); + } + } + // with the filter detector the pipeline check says why not + if (!obstacles_from_filter_) { + // its cells are NavCels of the finest size the map has (cell_size.min, now final) + memory_ = std::make_unique( + std::max(params_.min_size, 0.25), memory_min_time_, memory_max_hits_); + RCLCPP_INFO( + node->get_logger(), "[%s] obstacle memory (layer obstacle_hits): %s; a detection of %.1f s " + "over a NavCel is a hit, %d hits fix the obstacle (cells of %.2f m)", name.c_str(), + memory_enabled_at_start ? "on" : "off (can be switched on at runtime)", memory_min_time_, + memory_max_hits_, memory_->cell_size()); + for (const auto & h : saved_hits) { + memory_->restore(h[0], h[1], h[2], static_cast(h[3])); + } + if (!saved_hits.empty()) { + RCLCPP_INFO( + node->get_logger(), "[%s] obstacle memory restored from the saved map: %zu cells (%zu " + "fixed)", name.c_str(), memory_->cells().size(), memory_->fixed_count()); + } + } + map_frame_ = RTTFBuffer::getInstance()->get_tf_info().map_frame; + + const std::string base = node->get_node_base_interface()->get_fully_qualified_name() + + std::string("/") + name; + navmap_pub_ = node->create_publisher( + base + "/map", rclcpp::QoS(1).transient_local().reliable()); + create_services(base); + markers_pub_ = node->create_publisher( + base + "/markers", rclcpp::QoS(1).transient_local().reliable()); + + dynamic_pub_ = node->create_publisher( + base + "/dynamic_map", rclcpp::QoS(1).transient_local().reliable()); + layer_updates_pub_ = node->create_publisher( + base + "/map_updates", rclcpp::QoS(100)); + map_resolution_ = static_cast(surface_ ? params_.min_size : + mesher_->dem().resolution); + + // NavMap filters: the same plugins (and parameters) as in NavMapMapsManager. The `filters` list + // is the only on/off switch of each of them: a filter runs if and only if it is listed (and has + // a `.plugin`). Their instance names are free; what a filter is is told by the layer it + // writes. + std::vector loaded_filters; + filters_loader_ = std::make_unique>( + "easynav_navmap_maps_manager", "easynav::navmap::NavMapFilter"); + std::vector filter_names; + easynav::declare_parameter_if_absent(*node, name + ".filters", filter_names); + node->get_parameter(name + ".filters", filter_names); + for (const auto & fname : filter_names) { + std::string plugin; + easynav::declare_parameter_if_absent(*node, name + "." + fname + ".plugin", plugin); + node->get_parameter(name + "." + fname + ".plugin", plugin); + if (plugin.empty()) { + RCLCPP_WARN( + node->get_logger(), + "[%s] filter '%s' is listed in 'filters' but has no '%s.plugin' parameter: skipped%s", + name.c_str(), fname.c_str(), fname.c_str(), + fname == "obstacles" ? + " ('obstacles' is bigmap's own section: name an obstacle filter 'obstacle_filter')" : ""); + continue; + } + try { + auto instance = filters_loader_->createSharedInstance(plugin); + instance->initialize(node, name + "." + fname); + filters_.push_back(instance); + loaded_filters.push_back({fname, instance->get_layer_name()}); + RCLCPP_INFO( + node->get_logger(), "[%s] loaded NavMapFilter %s [%s] (layer '%s')", name.c_str(), + fname.c_str(), plugin.c_str(), instance->get_layer_name().c_str()); + } catch (const pluginlib::PluginlibException & ex) { + throw std::runtime_error( + "[" + name + "] unable to load NavMapFilter " + fname + " [" + plugin + "]: " + + ex.what()); + } + } + + { + // The options that decide how obstacles reach the planner depend on each other: check them + // together and say what is on (see pipeline_check.hpp) + bigmap_maps_manager::PipelineSettings settings; + settings.detector = detector; + settings.dynamic_map_enabled = dynamic_enabled_; + settings.memory_enabled = memory_enabled_at_start; + settings.removal_enabled = removal_enabled_; + settings.filters = loaded_filters; + const auto report = bigmap_maps_manager::check_pipeline(settings); + for (const auto & n : report.notes) { + RCLCPP_INFO(node->get_logger(), "[%s] %s", name.c_str(), n.c_str()); + } + for (const auto & w : report.warnings) { + RCLCPP_WARN(node->get_logger(), "[%s] %s", name.c_str(), w.c_str()); + } + if (!report.errors.empty()) { + throw std::invalid_argument("[" + name + "] " + report.errors.front()); + } + } + + + RCLCPP_INFO( + node->get_logger(), + "[%s] lengths (m): radii traversed_mark %.1f, traversed_full_res %.1f, " + "obstacle_clearance %.1f, " + "sensor_range %.1f, full_res %.1f, low_res_start %.1f, traversed_keep %.1f (%s); " + "cell_size %.2f..%.1f", name.c_str(), trail_mark_radius_, params_.known_margin, + removal_radius_, dyn_.max_range, params_.high_res_radius, params_.low_res_distance, + trail_keep_radius_, trail_persistent_ ? "unused: persistent" : "circles", params_.min_size, + params_.max_size); + if (surface_) { + RCLCPP_INFO( + node->get_logger(), "[%s] multi-level LOD radius %.0f m, clusters %.1f..%.0f m, imagery %s", + name.c_str(), params_.high_res_radius, params_.min_size, params_.max_size, + surface_->coloured() ? "yes" : "no"); + } else { + const auto & d = mesher_->dem(); + RCLCPP_INFO( + node->get_logger(), + "[%s] DEM %dx%d @ %.1f m (RTIN tile %d), LOD radius %.0f m, leaf %.1f..%.0f m, imagery %s", + name.c_str(), d.side, d.side, d.resolution, mesher_->error_map().tile_size(), + params_.high_res_radius, params_.min_size, params_.max_size, + d.rgb.empty() ? "no" : "yes"); + } + RCLCPP_INFO( + node->get_logger(), "[%s] obstacle borders seen from near kept fine: %s (within %.1f m, " + "%.1f m around them); hole error %.2f m; recovery on no path: %s; re-mesh in a worker: %s", + name.c_str(), observed_enabled_ ? "yes" : "no", observed_mark_radius_, + params_.observed_margin, hole_error, recovery_enabled_ ? "yes" : "no", + async_remesh_ ? "yes" : "no"); + watchdog_.start(wall_seconds()); + watchdog_timer_ = node->create_wall_timer( + std::chrono::seconds(5), [this]() {check_watchdog();}); +} + +void BigMapMapsManager::check_watchdog() +{ + auto node = get_node(); + const auto event = watchdog_.check(wall_seconds()); + if (event == bigmap_maps_manager::UpdateWatchdog::Event::None) {return;} + if (event == bigmap_maps_manager::UpdateWatchdog::Event::Resumed) { + RCLCPP_INFO(node->get_logger(), "[%s] update() runs again", get_plugin_name().c_str()); + return; + } + // not throttled on the ROS clock: that clock may be the one that does not move + const bool sim_time = node->get_parameter("use_sim_time").as_bool(); + const double ros_now = node->get_clock()->now().seconds(); + std::string why; + if (sim_time && ros_now == 0.0) { + why = ": the ROS clock of this node is still at 0 with use_sim_time true, so no simulated " + "time (/clock) has reached it, and EasyNav only runs a component when its clock says it is " + "due. Check that the simulation publishes /clock; if it does, restart the application"; + } else if (sim_time) { + why = ": the ROS clock of this node is at " + std::to_string(ros_now) + + " s (use_sim_time true): is the simulation paused or stopped?"; + } else { + why = ": EasyNav is not running the maps manager (is it active? is its cycle stuck?)"; + } + RCLCPP_WARN( + node->get_logger(), "[%s] update() has not run for %.0f s (wall time)%s", + get_plugin_name().c_str(), watchdog_.stalled_s(), why.c_str()); +} + +bool BigMapMapsManager::has_known() const +{ + if (surface_) {return surface_->traversed_count() > 0;} + return known_ && !known_->empty(); +} + +bool BigMapMapsManager::robot_position( + const ::easynav::NavState & nav_state, double & x, double & y, double & z, + std::array & q) +{ + // a NaN pose (a localizer that has not converged) would freeze re-meshing for good: it is no pose + const auto finite = [&]() { + return std::isfinite(x) && std::isfinite(y) && std::isfinite(z) && + std::isfinite(q[0]) && std::isfinite(q[1]) && std::isfinite(q[2]) && + std::isfinite(q[3]); + }; + if (nav_state.has("robot_pose")) { + const auto pose = nav_state.get_safe("robot_pose").pose.pose; + x = pose.position.x; y = pose.position.y; z = pose.position.z; + q = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w}; + return finite(); + } + try { + const auto & info = RTTFBuffer::getInstance()->get_tf_info(); + const auto tf = RTTFBuffer::getInstance()->lookupTransform( + info.map_frame, info.robot_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); + x = tf.transform.translation.x; + y = tf.transform.translation.y; + z = tf.transform.translation.z; + const auto & r = tf.transform.rotation; + q = {r.x, r.y, r.z, r.w}; + return finite(); + } catch (const tf2::TransformException &) { + return false; + } +} + +::navmap::NavMap BigMapMapsManager::build_navmap(const bigmap_maps_manager::LodMesh & mesh) const +{ + ::navmap::NavMap nm; + const std::size_t nv = mesh.x.size(), nt = mesh.triangle_count(); + nm.positions.x = mesh.x; + nm.positions.y = mesh.y; + nm.positions.z = mesh.z; + if (!mesh.r.empty()) { + ::navmap::Colors c; + c.r = mesh.r; c.g = mesh.g; c.b = mesh.b; + c.a.assign(nv, 255); + nm.colors = std::move(c); + } + nm.navcels.resize(nt); + nm.surfaces.resize(1); + nm.surfaces[0].frame_id = map_frame_; + nm.surfaces[0].navcels.resize(nt); + for (std::size_t t = 0; t < nt; ++t) { + nm.navcels[t].v[0] = mesh.tri[3 * t]; + nm.navcels[t].v[1] = mesh.tri[3 * t + 1]; + nm.navcels[t].v[2] = mesh.tri[3 * t + 2]; + nm.surfaces[0].navcels[t] = static_cast<::navmap::NavCelId>(t); + } + nm.rebuild_geometry_accels(); + + nm.add_layer( + "occupancy", "static map: terrain too steep to drive", "", ::navmap::FREE_SPACE); + nm.add_layer( + "obstacles", "occupancy plus what the sensors detect", "", ::navmap::FREE_SPACE); + nm.add_layer("elevation", "mean triangle height above the map origin", "m"); + nm.add_layer("lod_size", "leg length of the LOD triangle", "m"); + nm.add_layer( + "traversed", "255: ground the robot has driven over (high confidence)", "", 0); + nm.add_layer( + "obstacle_hits", "times an obstacle has been detected here (steep ground: obstacles.memory." + "max_hits); max_hits = fixed obstacle", "", 0); + const float min_nz = static_cast(std::cos(max_slope_deg_ * M_PI / 180.0)); + for (std::size_t t = 0; t < nt; ++t) { + const auto & v = nm.navcels[t].v; + const Eigen::Vector3f a(nm.positions.x[v[0]], nm.positions.y[v[0]], nm.positions.z[v[0]]); + const Eigen::Vector3f b(nm.positions.x[v[1]], nm.positions.y[v[1]], nm.positions.z[v[1]]); + const Eigen::Vector3f c(nm.positions.x[v[2]], nm.positions.y[v[2]], nm.positions.z[v[2]]); + const Eigen::Vector3f n = (b - a).cross(c - a).normalized(); + const auto id = static_cast<::navmap::NavCelId>(t); + // multi-level: the slope of the source NavCels (clustering makes the geometry steeper) + const bool steep = mesh.slope.size() == nt ? + mesh.slope[t] > static_cast(max_slope_deg_) : n.z() < min_nz; + if (steep) { + nm.layer_set("occupancy", id, ::navmap::LETHAL_OBSTACLE); + nm.layer_set("obstacles", id, ::navmap::LETHAL_OBSTACLE); + // too steep to drive is an obstacle for good: it counts as fixed + nm.layer_set("obstacle_hits", id, static_cast(memory_max_hits_)); + } + nm.layer_set("elevation", id, (a.z() + b.z() + c.z()) / 3.0f); + nm.layer_set("lod_size", id, mesh.leg[t]); + if (is_traversed(mesh, t)) {nm.layer_set("traversed", id, 255);} + } + return nm; +} + +bool BigMapMapsManager::memory_active() const +{ + if (!memory_) {return false;} + bool on = true; + const std::string key = get_plugin_name() + ".obstacles.memory.enabled"; + if (get_node()->has_parameter(key)) {get_node()->get_parameter(key, on);} + return on; +} + +void BigMapMapsManager::triangles_of( + const bigmap_maps_manager::ObstacleMemory::Cell & cell, std::vector & out) const +{ + // A memory cell is a square of `cell_size` metres (it was found by one detection, but the whole + // square is the obstacle): probe it finely enough to reach every NavCel of the mesh in it + const double size = memory_->cell_size(); + const double x0 = std::floor(cell.x / size) * size, y0 = std::floor(cell.y / size) * size; + const double step = std::max(0.1, 0.5 * params_.min_size); + const int n = std::max(1, static_cast(std::ceil(size / step))); + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + const int t = locator_->locate_at( + x0 + (i + 0.5) * size / n, y0 + (j + 0.5) * size / n, cell.z); + if (t >= 0) {out.push_back(static_cast(t));} + } + } +} + +void BigMapMapsManager::reset_hits(::navmap::NavMap & nm) const +{ + // what the layer holds with no memory: steep ground (an obstacle for good) and nothing else + const auto occ = std::dynamic_pointer_cast<::navmap::LayerView>( + nm.layers.get("occupancy")); + if (!occ || !nm.layers.get("obstacle_hits") || occ->size() != nm.navcels.size()) {return;} + for (std::size_t c = 0; c < nm.navcels.size(); ++c) { + const std::uint8_t v = occ->data()[c] == ::navmap::LETHAL_OBSTACLE ? + static_cast(memory_max_hits_) : 0; + const auto id = static_cast<::navmap::NavCelId>(c); + if (nm.layer_get("obstacle_hits", id, 0) != v) { + nm.layer_set("obstacle_hits", id, v); + } + } +} + +void BigMapMapsManager::fixed_seeds(std::vector & seeds) const +{ + // A fixed obstacle blocks the NavCels of its own size scale. Far from the robot the mesh is + // coarse: one of its NavCels can be tens of metres, and blocking it for a 1 m obstacle would + // close whole corridors to any plan made from afar. There it waits until the mesh around it is + // fine. + const double coarse = std::max(4.0 * params_.min_size, 2.0 * memory_->cell_size()); + std::vector tris; + for (const auto & kv : memory_->cells()) { + if (!memory_->fixed(kv.second)) {continue;} + tris.clear(); + triangles_of(kv.second, tris); + for (const auto t : tris) { + if (mesh_.leg[t] <= coarse) {seeds.push_back(t);} + } + } +} + +void BigMapMapsManager::write_hits(::navmap::NavMap & nm) const +{ + if (!memory_ || !locator_ || !nm.layers.get("obstacle_hits")) {return;} + std::vector tris; + for (const auto & kv : memory_->cells()) { + tris.clear(); + triangles_of(kv.second, tris); + for (const auto t : tris) { + const auto id = static_cast<::navmap::NavCelId>(t); + if (kv.second.hits > nm.layer_get("obstacle_hits", id, 0)) { + nm.layer_set("obstacle_hits", id, static_cast(kv.second.hits)); + } + } + } +} + +bool BigMapMapsManager::is_traversed(const bigmap_maps_manager::LodMesh & mesh, std::size_t t) const +{ + if (surface_) { + return trail_enabled_ && t < mesh.traversed.size() && mesh.traversed[t]; + } + if (!trail_enabled_ || !known_ || known_->empty()) {return false;} + const auto & dem = mesher_->dem(); + const int reach = static_cast(std::ceil(std::max(trail_mark_radius_, dem.resolution) / + dem.resolution)); + const int center = (dem.side - 1) / 2; + for (int k = 0; k < 3; ++k) { + const auto v = mesh.tri[3 * t + k]; + const int ix = static_cast(std::lround(mesh.x[v] / dem.resolution)) + center; + const int iy = static_cast(std::lround(mesh.y[v] / dem.resolution)) + center; + if (!known_->near_marked(ix, iy, reach)) {return false;} + } + return true; +} + +navmap_ros_interfaces::msg::NavMap BigMapMapsManager::make_map_msg( + const std::vector & removed, const ::navmap::NavMap & layers_from, + const rclcpp::Time & stamp, bool tint) const +{ + navmap_ros_interfaces::msg::NavMap msg; + msg.header.frame_id = map_frame_; + msg.header.stamp = stamp; + msg.positions_x = mesh_.x; + msg.positions_y = mesh_.y; + msg.positions_z = mesh_.z; + const bool imagery = !mesh_.r.empty(); + if (imagery) { + msg.colors_r = mesh_.r; + msg.colors_g = mesh_.g; + msg.colors_b = mesh_.b; + msg.colors_a.assign(mesh_.r.size(), 255); + } + // Obstacle NavCels (the `obstacles` layer, so the terrain too steep to drive and what the sensors + // found) are painted with a tint over the imagery. Vertices are shared between NavCels, so each + // of them gets its own copy of its three vertices to be coloured on its own. + // the uint8 layers of the NavMap in use, looked up once (not once per NavCel) + const auto u8 = [&layers_from, this](const char * lname) -> const std::vector * { + const auto view = std::dynamic_pointer_cast<::navmap::LayerView>( + layers_from.layers.get(lname)); + return view && view->size() == mesh_.triangle_count() ? &view->data() : nullptr; + }; + const auto * obstacles = u8("obstacles"); + auto lethal = [&](std::size_t t) { + return tint && tint_enabled_ && obstacles && (*obstacles)[t] == ::navmap::LETHAL_OBSTACLE; + }; + auto tinted = [&](std::uint8_t base, int channel) { + return static_cast(base + (tint_rgb_[channel] - base) * tint_strength_); + }; + std::vector kept; // original ids of the NavCels that stay + kept.reserve(mesh_.triangle_count()); + for (std::size_t t = 0; t < mesh_.triangle_count(); ++t) { + if (!removed.empty() && removed[t]) {continue;} + if (tint && view_max_z_ < 1e8) { // what is shown (a saved map keeps everything) + const auto a = mesh_.tri[3 * t], b = mesh_.tri[3 * t + 1], c = mesh_.tri[3 * t + 2]; + if ((mesh_.z[a] + mesh_.z[b] + mesh_.z[c]) / 3.0 > view_max_z_) {continue;} + } + kept.push_back(static_cast(t)); + if (!lethal(t)) { + msg.navcels_v0.push_back(mesh_.tri[3 * t]); + msg.navcels_v1.push_back(mesh_.tri[3 * t + 1]); + msg.navcels_v2.push_back(mesh_.tri[3 * t + 2]); + continue; + } + if (!imagery && msg.colors_r.empty()) { // no imagery: a neutral grey to tint + msg.colors_r.assign(msg.positions_x.size(), 170); + msg.colors_g = msg.colors_r; + msg.colors_b = msg.colors_r; + msg.colors_a.assign(msg.positions_x.size(), 255); + } + std::uint32_t id[3]; + for (int k = 0; k < 3; ++k) { + const auto v = mesh_.tri[3 * t + k]; + id[k] = static_cast(msg.positions_x.size()); + msg.positions_x.push_back(mesh_.x[v]); + msg.positions_y.push_back(mesh_.y[v]); + msg.positions_z.push_back(mesh_.z[v]); + const std::uint8_t r = imagery ? mesh_.r[v] : 170, g = imagery ? mesh_.g[v] : 170, + b = imagery ? mesh_.b[v] : 170; + msg.colors_r.push_back(tinted(r, 0)); + msg.colors_g.push_back(tinted(g, 1)); + msg.colors_b.push_back(tinted(b, 2)); + msg.colors_a.push_back(255); + } + msg.navcels_v0.push_back(id[0]); + msg.navcels_v1.push_back(id[1]); + msg.navcels_v2.push_back(id[2]); + } + msg.has_vertex_rgba = !msg.colors_r.empty(); + navmap_ros_interfaces::msg::NavMapSurface surf; + surf.frame_id = map_frame_; + surf.navcels.resize(kept.size()); + for (std::size_t k = 0; k < kept.size(); ++k) { + surf.navcels[k] = static_cast(k); + } + msg.surfaces.push_back(std::move(surf)); + + auto float_layer = [&](const std::string & lname, const std::string & desc, auto value) { + navmap_ros_interfaces::msg::NavMapLayer l; + l.header = msg.header; + l.name = lname; + l.type = navmap_ros_interfaces::msg::NavMapLayer::F32; + l.unit = "m"; + l.description = desc; + l.data_f32.resize(kept.size()); + for (std::size_t k = 0; k < kept.size(); ++k) { + l.data_f32[k] = value(kept[k]); + } + msg.layers.push_back(std::move(l)); + }; + float_layer( + "elevation", "mean triangle height above the map origin", [&](std::uint32_t t) { + return (mesh_.z[mesh_.tri[3 * t]] + mesh_.z[mesh_.tri[3 * t + 1]] + + mesh_.z[mesh_.tri[3 * t + 2]]) / 3.0f; + }); + float_layer("lod_size", "leg length of the LOD triangle", [&](std::uint32_t t) { + return mesh_.leg[t]; + }); + if (trail_enabled_ && has_known()) { + navmap_ros_interfaces::msg::NavMapLayer l; + l.header = msg.header; + l.name = "traversed"; + l.type = navmap_ros_interfaces::msg::NavMapLayer::U8; + l.description = "255: ground the robot has driven over (high confidence)"; + l.data_u8.resize(kept.size()); + for (std::size_t k = 0; k < kept.size(); ++k) { + l.data_u8[k] = is_traversed(mesh_, kept[k]) ? 255 : 0; + } + msg.layers.push_back(std::move(l)); + } + for (const char * lname : {"obstacles", "inflated_obstacles", "obstacle_hits"}) { + const auto * data = u8(lname); + if (!data) {continue;} + navmap_ros_interfaces::msg::NavMapLayer l; + l.header = msg.header; + l.name = lname; + l.type = navmap_ros_interfaces::msg::NavMapLayer::U8; + l.description = std::string(lname) == "obstacle_hits" ? + "times an obstacle was detected here (max_hits or more: fixed), snapshot when the map last " + "changed" : "cost layer (254 = lethal), snapshot when the map last changed"; + l.data_u8.resize(kept.size()); + for (std::size_t k = 0; k < kept.size(); ++k) { + l.data_u8[k] = (*data)[kept[k]]; + } + msg.layers.push_back(std::move(l)); + } + return msg; +} + +void BigMapMapsManager::publish_map( + const std::vector & removed, const ::navmap::NavMap & layers_from) +{ + if (!navmap_pub_) {return;} + const auto msg = make_map_msg(removed, layers_from, get_node()->now()); + if (tint_enabled_) { + RCLCPP_INFO_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 5000, + "[%s] obstacle NavCels painted: %zu of %zu", get_plugin_name().c_str(), + (msg.positions_x.size() - mesh_.x.size()) / 3, msg.navcels_v0.size()); + } + navmap_pub_->publish(msg); +} + +void BigMapMapsManager::restore_removed(::easynav::NavState & nav_state) +{ + if (removed_.empty()) {return;} + const auto map = nav_state.get_ptr<::navmap::NavMap>("map.navmap"); + auto & nm = *map; + const auto occ = std::dynamic_pointer_cast<::navmap::LayerView>( + nm.layers.get("occupancy")); + if (!nm.layers.get("obstacles") || !occ || occ->size() != nm.navcels.size()) {return;} + for (const auto c : removed_) { + nm.layer_set( + "obstacles", static_cast<::navmap::NavCelId>(c), occ->data()[c]); + } +} + +void BigMapMapsManager::apply_removal( + ::easynav::NavState & nav_state, const std::vector & seeds) +{ + const auto map = nav_state.get_ptr<::navmap::NavMap>("map.navmap"); + auto & nm = *map; + const auto obs = std::dynamic_pointer_cast<::navmap::LayerView>( + nm.layers.get("obstacles")); + const auto occ = std::dynamic_pointer_cast<::navmap::LayerView>( + nm.layers.get("occupancy")); + const std::size_t n = nm.navcels.size(); + if (!obs || !occ || obs->size() != n || centroid_x_.size() != n) {return;} + const auto & base = occ->data(); + + // The NavCels with an obstacle and all those whose centre is within the radius of one of them. + // With the obstacle memory on, each of them also shows in `obstacle_hits` the count of the + // obstacle it belongs to (a red NavCel is an obstacle: it must have its place in that layer) + const bool hits_on = memory_ && memory_was_on_ && nm.layers.get("obstacle_hits"); + std::vector ring_hits(hits_on ? n : 0, 0); + auto hits_of = [&](std::uint32_t c) { + return nm.layer_get("obstacle_hits", static_cast<::navmap::NavCelId>(c), 0); + }; + std::vector in_removed(n, 0); + for (const auto s : seeds) { + in_removed[s] = 1; + if (hits_on) {ring_hits[s] = std::max(ring_hits[s], hits_of(s));} + } + if (removal_radius_ > 0.0 && !seeds.empty()) { + // the centroids within the radius, through the grid of centroids built with the mesh + // (cells >= the radius: the 3 x 3 cells around a seed hold all of them) + const float r2 = static_cast(removal_radius_ * removal_radius_); + for (const auto s : seeds) { + const auto sx = static_cast(std::floor(centroid_x_[s] / centroid_cell_)); + const auto sy = static_cast(std::floor(centroid_y_[s] / centroid_cell_)); + for (std::int64_t j = sy - 1; j <= sy + 1; ++j) { + for (std::int64_t i = sx - 1; i <= sx + 1; ++i) { + const auto it = centroid_grid_.find(grid_key(i, j)); + if (it == centroid_grid_.end()) {continue;} + for (const auto c : it->second) { + const float dx = centroid_x_[c] - centroid_x_[s], dy = centroid_y_[c] - centroid_y_[s]; + // another floor over the obstacle is not next to it + if (dx * dx + dy * dy <= r2 && std::abs(centroid_z_[c] - centroid_z_[s]) <= 1.0f) { + in_removed[c] = 1; + if (hits_on) {ring_hits[c] = std::max(ring_hits[c], hits_of(s));} + } + } + } + } + } + } + + std::vector now_removed; + for (std::size_t c = 0; c < n; ++c) { + if (in_removed[c]) {now_removed.push_back(static_cast(c));} + } + // released NavCels get their base value back; the removed ones are blocked for the planner + for (const auto c : removed_) { + if (!in_removed[c]) { + nm.layer_set("obstacles", static_cast<::navmap::NavCelId>(c), base[c]); + // no longer next to an obstacle: its count goes (steep ground keeps its own; the memory's + // cells are written back by write_hits) + if (hits_on && base[c] != ::navmap::LETHAL_OBSTACLE && hits_of(c) > 0) { + nm.layer_set("obstacle_hits", static_cast<::navmap::NavCelId>(c), 0); + } + } + } + if (hits_on) { + for (const auto c : now_removed) { + if (ring_hits[c] > hits_of(c)) { + nm.layer_set( + "obstacle_hits", static_cast<::navmap::NavCelId>(c), ring_hits[c]); + } + } + } + const auto & o = obs->data(); + for (const auto c : now_removed) { + if (o[c] != ::navmap::LETHAL_OBSTACLE) { + nm.layer_set( + "obstacles", static_cast<::navmap::NavCelId>(c), ::navmap::LETHAL_OBSTACLE); + } + } + if (now_removed != removed_) { + removed_ = now_removed; + // published at the end of update(), when the inflation of this very cycle is done: the map + // carries its layers, and published here it would show the previous cycle's inflated_obstacles + pending_flags_ = in_removed; + publish_pending_ = true; + } +} + +void BigMapMapsManager::apply_obstacle_removal(::easynav::NavState & nav_state) +{ + // obstacle_source: filter -> the NavCels the ObstaclesFilter marked (not the terrain's own) + const auto map = nav_state.get_ptr<::navmap::NavMap>("map.navmap"); + auto & nm = *map; + const auto obs = std::dynamic_pointer_cast<::navmap::LayerView>( + nm.layers.get("obstacles")); + const auto occ = std::dynamic_pointer_cast<::navmap::LayerView>( + nm.layers.get("occupancy")); + const std::size_t n = nm.navcels.size(); + if (!obs || !occ || obs->size() != n) {return;} + std::vector seeds; + for (std::size_t c = 0; c < n; ++c) { + if (obs->data()[c] == ::navmap::LETHAL_OBSTACLE && + occ->data()[c] != ::navmap::LETHAL_OBSTACLE) + { + seeds.push_back(static_cast(c)); + } + } + apply_removal(nav_state, seeds); +} + +navmap_ros_interfaces::msg::NavMap BigMapMapsManager::make_dynamic_msg( + const std::vector & tiles, const rclcpp::Time & stamp) const +{ + navmap_ros_interfaces::msg::NavMap msg; + msg.header.frame_id = map_frame_; + msg.header.stamp = stamp; + const bool coloured = !mesh_.r.empty(); + msg.has_vertex_rgba = coloured; + navmap_ros_interfaces::msg::NavMapSurface surf; + surf.frame_id = map_frame_; + navmap_ros_interfaces::msg::NavMapLayer elevation, height, size; + for (auto * l : {&elevation, &height, &size}) { + l->header = msg.header; + l->type = navmap_ros_interfaces::msg::NavMapLayer::F32; + l->unit = "m"; + } + elevation.name = "elevation"; + height.name = "obstacle_height"; + height.description = "height of the NavCel above the terrain, as measured by the sensors"; + size.name = "lod_size"; + for (std::size_t k = 0; k < tiles.size(); ++k) { + const auto & t = tiles[k]; + // own vertices (not shared with the terrain): the tile is detached and raised as a whole + for (int c = 0; c < 3; ++c) { + const auto v = mesh_.tri[3 * t.triangle + c]; + msg.positions_x.push_back(mesh_.x[v]); + msg.positions_y.push_back(mesh_.y[v]); + msg.positions_z.push_back(t.z_top); + if (coloured) { + msg.colors_r.push_back(mesh_.r[v]); + msg.colors_g.push_back(mesh_.g[v]); + msg.colors_b.push_back(mesh_.b[v]); + msg.colors_a.push_back(255); + } + } + const auto base = static_cast(3 * k); + msg.navcels_v0.push_back(base); + msg.navcels_v1.push_back(base + 1); + msg.navcels_v2.push_back(base + 2); + surf.navcels.push_back(static_cast(k)); + elevation.data_f32.push_back(t.z_top); + height.data_f32.push_back(t.above_surface); + size.data_f32.push_back(mesh_.leg[t.triangle]); + } + msg.surfaces.push_back(std::move(surf)); + msg.layers = {elevation, height, size}; + return msg; +} + +void BigMapMapsManager::update_dynamic_map( + ::easynav::NavState & nav_state, double x, double y, double z, + const std::array & q) +{ + const double now = get_node()->now().seconds(); + // Multi-level: there is no single height under an (x, y); the NavCels (picked by the robot's + // height) are the reference, and the robot's own z only backs up a miss + const bigmap_maps_manager::DynamicLayer::Surface terrain = surface_ ? + bigmap_maps_manager::DynamicLayer::Surface([z](double, double) {return z;}) : + bigmap_maps_manager::DynamicLayer::Surface( + [dem = &mesher_->dem()](double px, double py) {return dem->height_at(px, py);}); + + const auto & perceptions = nav_state.get_no_group(); + if (!perceptions.empty()) { + try { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + const float r = static_cast(dyn_.max_range); + // in the robot's footprint frame (z = 0 at the ground contact): the heights are measured + // relative to the robot, not through the map -> robot TF, whose z is not reliable + std::vector> pts; + { + bigmap_maps_manager::ProfScope s(prof(), "dyn: sensor clouds (total)"); + // Cut to the box around the robot FIRST and thin the survivors afterwards: the voxel + // filter hashes every point it is given, and a depth camera gives hundreds of thousands, + // nearly all of them outside the box (that order took ~47 ms per cycle, the cut is cheap) + PointPerceptionsOpsView view(perceptions); + { + bigmap_maps_manager::ProfScope sf(prof(), "dyn: fuse (TF lookups)"); + view.fuse(tf_info.robot_footprint_frame); + } + { + bigmap_maps_manager::ProfScope sf(prof(), "dyn: filter (cut to the box)"); + view.filter({-r, -r, NAN}, {r, r, static_cast(dyn_.max_height)}, false); + } + { + bigmap_maps_manager::ProfScope sf(prof(), "dyn: downsample (voxel)"); + view.downsample(dyn_.voxel); + } + pcl::PointCloud cloud; + { + bigmap_maps_manager::ProfScope sf(prof(), "dyn: as_points"); + cloud = view.as_points(); + } + pts.reserve(cloud.points.size()); + for (const auto & p : cloud.points) { + pts.push_back({p.x, p.y, p.z}); + } + } + bigmap_maps_manager::ProfScope s_ingest(prof(), "dyn: ingest (cells, plane fit)"); + if (locator_ && + dyn_layer_.ingest(pts, x, y, q, now, terrain, dyn_, &mesh_, locator_.get(), z)) + { + dyn_dirty_ = true; + } + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 5000, + "[%s] dynamic map: %s", get_plugin_name().c_str(), e.what()); + } + } + { + bigmap_maps_manager::ProfScope s(prof(), "dyn: expire"); + if (dyn_layer_.expire(now, dyn_)) {dyn_dirty_ = true;} + } + + // Memory of obstacles: every min_time s a detection goes on over a spot is a hit there + const bool memory_on = memory_active(); + if (clear_obstacles_requested_.exchange(false) && memory_) { + const std::size_t n = memory_->cells().size(), fixed = memory_->fixed_count(); + memory_->clear(); + if (nav_state.has("map.navmap")) { + reset_hits(*nav_state.get_ptr<::navmap::NavMap>("map.navmap")); + } + dyn_dirty_ = true; // the fixed obstacles are released below + RCLCPP_INFO( + get_node()->get_logger(), "[%s] obstacle memory cleared: %zu cells (%zu fixed) forgotten", + get_plugin_name().c_str(), n, fixed); + } + if (memory_on) { + const auto hits = dyn_layer_.sustained(now, memory_min_time_); + if (!hits.empty()) { + memory_->add(hits, now); + dyn_dirty_ = true; // a new obstacle (5 s over a NavCel) or a new hit + } + } + if (memory_on != memory_was_on_) { + // switched off: the counts stay in the memory, but the layer stops showing them (they no + // longer block) and the fixed obstacles are released below (dirty) + if (!memory_on && nav_state.has("map.navmap")) { + reset_hits(*nav_state.get_ptr<::navmap::NavMap>("map.navmap")); + } + memory_was_on_ = memory_on; + dyn_dirty_ = true; + } + + // at most 5 Hz, and only when something changed + if (!dyn_dirty_ || !locator_ || now - last_dyn_publish_ < 0.2) {return;} + dyn_dirty_ = false; + last_dyn_publish_ = now; + // every confirmed detection: the dynamic map, and the obstacles when the memory is off + const bool want_tiles = dynamic_enabled_ || !memory_on; + const auto tiles = want_tiles ? dyn_layer_.tiles(mesh_, *locator_) : + std::vector{}; + if (removal_enabled_ && !obstacles_from_filter_) { + bigmap_maps_manager::ProfScope s(prof(), "dyn: remove NavCels (apply_removal)"); + // what the sensors measured above the terrain (relative to the robot: no TF z involved) + // is what is removed from the map and blocked for the planner. With the memory on, only what + // has been over a NavCel for min_time s counts, and fixed obstacles (max_hits) stay for good + std::vector seeds; + if (memory_on) { + if (nav_state.has("map.navmap")) { + write_hits(*nav_state.get_ptr<::navmap::NavMap>("map.navmap")); + } + for (const auto & t : dyn_layer_.tiles(mesh_, *locator_, memory_min_time_)) { + seeds.push_back(t.triangle); + } + fixed_seeds(seeds); + std::sort(seeds.begin(), seeds.end()); + seeds.erase(std::unique(seeds.begin(), seeds.end()), seeds.end()); + } else { + for (const auto & t : tiles) { + seeds.push_back(t.triangle); + } + } + apply_removal(nav_state, seeds); + } + if (memory_on && nav_state.has("map.navmap")) { + write_hits(*nav_state.get_ptr<::navmap::NavMap>("map.navmap")); + } + if (!dynamic_enabled_) {return;} + bigmap_maps_manager::ProfScope s(prof(), "dyn: publish dynamic map"); + auto msg = make_dynamic_msg(tiles, get_node()->now()); + dynamic_pub_->publish(msg); + nav_state.set( + "map.dynamic", tiles.empty() ? ::navmap::NavMap() : navmap_ros::from_msg(msg)); +} + +void BigMapMapsManager::release_sensor_obstacles(::easynav::NavState & nav_state) +{ + // Nothing analyses the sensors (dynamic_map and memory both off, e.g. a recovery switched the + // memory off): what they blocked, fixed obstacles included, is released at once + if (memory_was_on_) { + reset_hits(*nav_state.get_ptr<::navmap::NavMap>("map.navmap")); + memory_was_on_ = false; + } + if (!removed_.empty() && removal_enabled_ && !obstacles_from_filter_) { + apply_removal(nav_state, {}); + } +} + +void BigMapMapsManager::process_save(::easynav::NavState & nav_state) +{ + std::shared_ptr req; + { + std::lock_guard lock(save_mutex_); + req = pending_save_; + pending_save_.reset(); + } + if (!req) {return;} + try { + if (mesh_.triangle_count() == 0) {throw std::runtime_error("no map yet");} + // the layers (obstacles...) as the NavState holds them now + const auto nav_map = nav_state.has("map.navmap") ? + nav_state.get_ptr<::navmap::NavMap>("map.navmap") : nullptr; + const ::navmap::NavMap & layers = nav_map ? *nav_map : navmap_; + std::vector removed; + if (req->traversed_only) { + removed.assign(mesh_.triangle_count(), 1); + std::size_t n = 0; + for (std::size_t t = 0; t < mesh_.triangle_count(); ++t) { + if (is_traversed(mesh_, t)) {removed[t] = 0; ++n;} + } + if (n == 0) {throw std::runtime_error("the robot has not driven over any ground yet");} + } + // untinted: the saved colours are the imagery's + const auto msg = make_map_msg(removed, layers, get_node()->now(), false); + std::filesystem::create_directories(save_directory_); + const std::string path = req->path; + if (!navmap_ros::io::save_to_file(navmap_ros::from_msg(msg), path)) { + throw std::runtime_error("could not write " + path); + } + bigmap_maps_manager::save_origin(path, origin_, params_.min_size); + RCLCPP_INFO( + get_node()->get_logger(), "[%s] saved %zu NavCels to %s", get_plugin_name().c_str(), + msg.navcels_v0.size(), path.c_str()); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_node()->get_logger(), "[%s] could not save %s: %s", get_plugin_name().c_str(), + req->path.c_str(), e.what()); + } +} + +void BigMapMapsManager::mark_trail(double x, double y, double z) +{ + if (surface_) { + surface_->mark(x, y, z, trail_mark_radius_); + } else if (known_) { + known_->mark(x, y, trail_mark_radius_); + } +} + +void BigMapMapsManager::mark_observed(double x, double y, double z) +{ + if (!observed_enabled_) {return;} + if (surface_) { + surface_->mark_observed(x, y, z, observed_mark_radius_); + return; + } + if (!observed_) {return;} + // the nodes on the border of a hole of the map (some of their cells have ground, some not) + // within the radius + const auto & cv = mesher_->cell_valid(); + const auto & dem = mesher_->dem(); + const int side = dem.side, nc = side - 1, center = (side - 1) / 2; + const double res = dem.resolution; + const int reach = static_cast(std::ceil(observed_mark_radius_ / res)); + const int cx = static_cast(std::lround(x / res)) + center; + const int cy = static_cast(std::lround(y / res)) + center; + for (int j = std::max(0, cy - reach); j <= std::min(side - 1, cy + reach); ++j) { + for (int i = std::max(0, cx - reach); i <= std::min(side - 1, cx + reach); ++i) { + if (std::hypot((i - center) * res - x, (j - center) * res - y) > observed_mark_radius_) { + continue; + } + bool some = false, none = false; + for (int b = j - 1; b <= j; ++b) { + for (int a = i - 1; a <= i; ++a) { + if (a < 0 || b < 0 || a >= nc || b >= nc) {continue;} + (cv[static_cast(b) * nc + a] ? some : none) = true; + } + } + if (some && none) {observed_->mark_node(i, j);} + } + } +} + +void BigMapMapsManager::prepare_remesh(double x, double y, double z) +{ + mark_observed(x, y, z); + if (surface_) { + if (trail_enabled_ && !trail_persistent_) { + surface_->forget_outside(x, y, z, trail_keep_radius_); + } + if (!trail_persistent_) {surface_->forget_observed_outside(x, y, z, trail_keep_radius_);} + return; + } + if (trail_enabled_ && known_) { + bigmap_maps_manager::ProfScope s(prof(), "remesh: traversed ground (tree of nodes)"); + if (!trail_persistent_) { + // beyond the large circle the ground driven over is forgotten (as resolution and as the + // `traversed` layer) until the robot drives over it again + known_->forget_outside(x, y, trail_keep_radius_); + } + known_->update(); + } + if (observed_) { + if (!trail_persistent_) {observed_->forget_outside(x, y, trail_keep_radius_);} + observed_->update(); + } +} + +BigMapMapsManager::Remesh BigMapMapsManager::make_remesh( + double x, double y, double z, const bigmap_maps_manager::LodParams & params, + bigmap_maps_manager::Profiler * prof) const +{ + Remesh r; + r.t0 = std::chrono::steady_clock::now(); + r.x = x; + r.y = y; + r.z = z; + { + bigmap_maps_manager::ProfScope s(prof, "remesh: extract (RTIN)"); + r.mesh = surface_ ? surface_->extract(x, y, z, params) : + mesher_->extract(x, y, params, trail_enabled_ ? known_.get() : nullptr, observed_.get()); + } + { + bigmap_maps_manager::ProfScope s(prof, "remesh: build NavMap"); + r.navmap = build_navmap(r.mesh); + } + return r; +} + +void BigMapMapsManager::check_recovery(const ::easynav::NavState & nav_state) +{ + // With a goal and no path for recovery_cycles_ cycles the map may be too coarse where the way + // is (a door, a gap between obstacles merged into one): the full-resolution circle is doubled, + // up to recovery_max_factor_ times its value, and the map re-meshed. Back to normal when there + // is no goal any more (reached or dropped): changing it while a path exists could lose it. + const bool has_goal = nav_state.has("goals") && + !nav_state.get("goals").goals.empty(); + const bool has_path = nav_state.has("path") && + !nav_state.get("path").poses.empty(); + if (!has_goal) { + no_path_cycles_ = 0; + if (params_.high_res_radius != base_high_res_) { + params_.high_res_radius = base_high_res_; + params_.low_res_distance = base_low_res_; + force_remesh_ = true; + RCLCPP_INFO( + get_node()->get_logger(), "[%s] recovery over: full resolution back to %.1f m", + get_plugin_name().c_str(), base_high_res_); + } + return; + } + if (has_path) {no_path_cycles_ = 0; return;} + if (++no_path_cycles_ < recovery_cycles_) {return;} + no_path_cycles_ = 0; + const double cap = base_high_res_ * recovery_max_factor_; + if (params_.high_res_radius >= cap) { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 10000, + "[%s] no path to the goal even at full resolution within %.1f m", get_plugin_name().c_str(), + params_.high_res_radius); + return; + } + params_.high_res_radius = std::min(cap, std::max(params_.high_res_radius * 2.0, 1.0)); + params_.low_res_distance = std::max(base_low_res_, params_.high_res_radius); + force_remesh_ = true; + RCLCPP_WARN( + get_node()->get_logger(), "[%s] no path to the goal for %d cycles: full resolution within " + "%.1f m (recovery)", get_plugin_name().c_str(), recovery_cycles_, params_.high_res_radius); +} + +void BigMapMapsManager::commit_remesh(Remesh && r) +{ + const double x = r.x, y = r.y, z = r.z; + const auto t0 = r.t0; + auto & new_mesh = r.mesh; + auto & new_navmap = r.navmap; + // the locator (which points into mesh_) is dropped before and rebuilt after + locator_.reset(); + mesh_ = std::move(new_mesh); + navmap_ = std::move(new_navmap); + const auto & mesh = mesh_; + map_changed_ = true; + + // NavCel ids change with every mesh: the previous removals do not apply any more + removed_.clear(); + publish_pending_ = false; + { + bigmap_maps_manager::ProfScope s(prof(), "remesh: locator + centroids"); + locator_ = std::make_unique(mesh_); + dyn_dirty_ = true; // NavCel ids changed: the raised ones must be found again + last_dyn_publish_ = -1e9; // and at once, not after the 5 Hz throttle + centroid_x_.resize(mesh.triangle_count()); + centroid_y_.resize(mesh.triangle_count()); + centroid_z_.resize(mesh.triangle_count()); + for (std::size_t t = 0; t < mesh.triangle_count(); ++t) { + const auto a = mesh.tri[3 * t], b = mesh.tri[3 * t + 1], c = mesh.tri[3 * t + 2]; + centroid_x_[t] = (mesh.x[a] + mesh.x[b] + mesh.x[c]) / 3.0f; + centroid_y_[t] = (mesh.y[a] + mesh.y[b] + mesh.y[c]) / 3.0f; + centroid_z_[t] = (mesh.z[a] + mesh.z[b] + mesh.z[c]) / 3.0f; + } + // grid of the centroids for the clearance ring of apply_removal + centroid_cell_ = std::max(removal_radius_, 0.5); + centroid_grid_.clear(); + for (std::size_t t = 0; t < mesh.triangle_count(); ++t) { + centroid_grid_[grid_key( + static_cast(std::floor(centroid_x_[t] / centroid_cell_)), + static_cast(std::floor(centroid_y_[t] / centroid_cell_)))].push_back( + static_cast(t)); + } + if (memory_active()) { + write_hits(navmap_); // what was detected before this mesh, on its NavCels + } + } + + const auto stamp = get_node()->now(); + { + bigmap_maps_manager::ProfScope s(prof(), "remesh: publish map"); + publish_map({}, navmap_); + } + if (publish_markers_) { + bigmap_maps_manager::ProfScope s(prof(), "remesh: markers"); + markers_pub_->publish( + bigmap_maps_manager::make_markers( + mesh, params_, x, y, z, params_.max_size, map_frame_, stamp, + trail_enabled_ && !trail_persistent_ ? trail_keep_radius_ : 0.0)); + } + std::size_t traversed = 0; + { + bigmap_maps_manager::ProfScope s(prof(), "remesh: count traversed (log)"); + for (std::size_t t = 0; t < mesh.triangle_count(); ++t) { + traversed += is_traversed(mesh, t); + } + } + RCLCPP_INFO_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 5000, + "[%s] LOD @ (%.1f, %.1f): %zu triangles (%zu on traversed ground) in %.2f ms", + get_plugin_name().c_str(), x, y, mesh.triangle_count(), traversed, + std::chrono::duration(std::chrono::steady_clock::now() - t0).count()); +} + +void BigMapMapsManager::update(::easynav::NavState & nav_state) +{ + watchdog_.ran(wall_seconds()); + bigmap_maps_manager::ProfScope total_scope(prof(), "update (total)"); + double x = 0.0, y = 0.0, z = 0.0; + std::array q = {0.0, 0.0, 0.0, 1.0}; + bool have_robot = false; + { + bigmap_maps_manager::ProfScope s(prof(), "pose + mark"); + have_robot = robot_position(nav_state, x, y, z, q); + if (have_robot && trail_enabled_) { + if (remesh_job_.valid()) { + pending_marks_.push_back({x, y, z}); // the worker reads the ground: marked after it + } else { + mark_trail(x, y, z); + } + } + } + + if (!have_robot) { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 10000, + "[%s] no robot pose yet (neither 'robot_pose' from the localizer nor TF map -> base_link): " + "the map is not published until there is one", get_plugin_name().c_str()); + } + if (recovery_enabled_) {check_recovery(nav_state);} + + // A re-mesh the worker finished is committed first + if (remesh_job_.valid() && + remesh_job_.wait_for(std::chrono::seconds(0)) == std::future_status::ready) + { + try { + commit_remesh(remesh_job_.get()); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_node()->get_logger(), "[%s] re-mesh failed, the previous map stays: %s", + get_plugin_name().c_str(), e.what()); + } + for (const auto & m : pending_marks_) { + mark_trail(m[0], m[1], m[2]); + } + pending_marks_.clear(); + } + + // Re-mesh on the first valid pose, then whenever the robot moved enough (one at a time) + if (have_robot && !remesh_job_.valid() && + (force_remesh_ || !last_meshed_ || + std::hypot(x - last_meshed_->first, y - last_meshed_->second) >= hysteresis_ || + (surface_ && std::abs(z - last_meshed_z_) >= 1.0))) + { + force_remesh_ = false; + bigmap_maps_manager::ProfScope remesh_scope(prof(), "REMESH (total)"); + prepare_remesh(x, y, z); + last_meshed_ = {x, y}; + last_meshed_z_ = z; + if (async_remesh_ && navmap_.navcels.size() > 0) { + // (the first mesh is made here: there is nothing to show until it exists) + remesh_job_ = std::async( + std::launch::async, [this, x, y, z, params = params_]() { + return make_remesh(x, y, z, params, nullptr); + }); + } else { + // built aside and committed together: an exception while building leaves the previous + // mesh and NavMap + commit_remesh(make_remesh(x, y, z, params_, prof())); + } + } + + // (no map before the first pose: an empty NavMap would only mislead the filters and the planner) + if (map_changed_ || (!nav_state.has("map.navmap") && last_meshed_)) { + bigmap_maps_manager::ProfScope s(prof(), "set map.navmap (copy)"); + nav_state.set("map.navmap", navmap_); + map_changed_ = false; + } + + process_save(nav_state); + + { + // view_max_z changed at runtime (ros2 param set): the map RViz shows is published again + double wanted = view_max_z_; + const std::string vkey = get_plugin_name() + ( + get_node()->has_parameter(get_plugin_name() + ".output.view_max_z") ? + ".output.view_max_z" : ".view_max_z"); + get_node()->get_parameter(vkey, wanted); + if (wanted != view_max_z_ && nav_state.has("map.navmap")) { + view_max_z_ = wanted; + std::vector flags(mesh_.triangle_count(), 0); + for (const auto c : removed_) { + if (c < flags.size()) {flags[c] = 1;} + } + publish_map(flags, nav_state.get<::navmap::NavMap>("map.navmap")); + RCLCPP_INFO( + get_node()->get_logger(), "[%s] RViz shows the map up to z = %.2f m", + get_plugin_name().c_str(), view_max_z_); + } + } + + if (!nav_state.has("map.navmap")) {return;} + + // Sensor obstacles BEFORE the filters, so that the inflation (what A* reads) is built on this + // cycle's obstacles, also right after a re-mesh, when the NavState map has just been replaced + if (have_robot) { + if (dynamic_enabled_ || memory_active()) { + bigmap_maps_manager::ProfScope s(prof(), "DYNAMIC MAP (total)"); + update_dynamic_map(nav_state, x, y, z, q); + } else { + release_sensor_obstacles(nav_state); + } + } + + // Filters edit the NavMap held by the NavState (obstacles seen by the sensors, inflation) + // With removal the published map has holes, so per-NavCel layer updates (indexed on the + // complete mesh) would not match it: the layers travel inside the map message instead + const std::size_t subscribers = + removal_enabled_ ? 0 : layer_updates_pub_->get_subscription_count(); + const bool new_subscriber = subscribers > last_subscribers_; + last_subscribers_ = subscribers; + std_msgs::msg::Header header; + header.frame_id = map_frame_; + header.stamp = get_node()->now(); + for (const auto & filter : filters_) { + bigmap_maps_manager::ProfScope filter_scope(prof(), "filters"); + filter->set_map_resolution(map_resolution_); + // Take back our own marks first, so that the filter (and our seeds) only see the sensors', + // not the ring we blocked in the previous cycle + if (removal_enabled_ && obstacles_from_filter_ && filter->get_layer_name() == "obstacles") { + restore_removed(nav_state); + } + filter->update(nav_state); + if (removal_enabled_ && obstacles_from_filter_ && filter->get_layer_name() == "obstacles") { + apply_obstacle_removal(nav_state); // before the inflation filter, which builds on it + } + if (subscribers == 0 || !filter->is_adding_layer()) {continue;} + const auto & nm = nav_state.get<::navmap::NavMap>("map.navmap"); + const auto layer = nm.layers.get(filter->get_layer_name()); + if (!layer || (!layer->has_changes() && !new_subscriber)) {continue;} + layer_updates_pub_->publish( + navmap_ros::to_layer_update_msg( + nm, filter->get_layer_name(), header, new_subscriber ? 0.0f : 0.25f)); + } + const auto & nm = nav_state.get<::navmap::NavMap>("map.navmap"); + if (publish_pending_) { + publish_pending_ = false; + publish_map(pending_flags_, nm); + } + for (const auto & lname : nm.list_layers()) { + nm.layers.get(lname)->clear_changes(); + } + if (profile_enabled_ && profiler_.due()) { + RCLCPP_INFO( + get_node()->get_logger(), "[%s] %s", get_plugin_name().c_str(), + profiler_.report_and_reset("timing of update()").c_str()); + } +} + +} // namespace bigmap +} // namespace easynav + +PLUGINLIB_EXPORT_CLASS(easynav::bigmap::BigMapMapsManager, easynav::MapsManagerBase) diff --git a/maps_managers/easynav_bigmap_maps_manager/src/dem_grid.cpp b/maps_managers/easynav_bigmap_maps_manager/src/dem_grid.cpp new file mode 100644 index 00000000..f00bb8f4 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/dem_grid.cpp @@ -0,0 +1,191 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/dem_grid.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace bigmap_maps_manager +{ + +namespace +{ +constexpr char kMagic[8] = {'L', 'M', 'D', 'E', 'M', '0', '0', '6'}; +// the optional block after the colours: which cells have ground (DemGrid::cell_valid) +constexpr char kCellsTag[8] = {'C', 'E', 'L', 'L', 'S', '\0', '\0', '\0'}; +} // namespace + +double DemGrid::height_at(double x, double y) const +{ + // a NaN survives std::clamp and its cast to int is undefined (INT_MIN on x86: a read far + // outside z): no height there + if (!std::isfinite(x) || !std::isfinite(y)) {return std::numeric_limits::quiet_NaN();} + const double c = (side - 1) / 2.0; + const double u = std::clamp(x / resolution + c, 0.0, side - 1.0); + const double v = std::clamp(y / resolution + c, 0.0, side - 1.0); + const int i = std::min(static_cast(u), side - 2), + j = std::min(static_cast(v), side - 2); + const double tu = u - i, tv = v - j; + return (1 - tv) * ((1 - tu) * at(i, j) + tu * at(i + 1, j)) + + tv * ((1 - tu) * at(i, j + 1) + tu * at(i + 1, j + 1)); +} + +DemGrid DemGrid::subsample(int stride) const +{ + if (stride < 1 || (side - 1) % stride != 0) { + throw std::invalid_argument("stride must divide side - 1"); + } + DemGrid g = *this; + g.side = (side - 1) / stride + 1; + g.resolution = resolution * stride; + g.rgb.clear(); + g.imagery_zoom = 0; + g.z.resize(static_cast(g.side) * g.side); + for (int j = 0; j < g.side; ++j) { + for (int i = 0; i < g.side; ++i) { + g.z[static_cast(j) * g.side + i] = at(i * stride, j * stride); + } + } + if (!cell_valid.empty()) { + const int nc = side - 1, gc = g.side - 1; + g.cell_valid.assign(static_cast(gc) * gc, 1); + for (int j = 0; j < nc; ++j) { + for (int i = 0; i < nc; ++i) { + if (!cell_valid[static_cast(j) * nc + i]) { + g.cell_valid[static_cast(j / stride) * gc + i / stride] = 0; + } + } + } + } + return g; +} + +bool DemGrid::save(const std::string & path) const +{ + std::error_code ec; + const auto parent = std::filesystem::path(path).parent_path(); + if (!parent.empty()) {std::filesystem::create_directories(parent, ec);} + const std::string tmp = path + ".tmp"; + std::ofstream f(tmp, std::ios::binary | std::ios::trunc); + if (!f) {return false;} + const std::int32_t s = side; + const std::int32_t syn = source; + const double hdr[7] = {resolution, radius, origin_lat, origin_lon, reference_altitude, + requested_reference, data_resolution}; + f.write(kMagic, sizeof(kMagic)); + f.write(reinterpret_cast(&s), sizeof(s)); + f.write(reinterpret_cast(&syn), sizeof(syn)); + f.write(reinterpret_cast(hdr), sizeof(hdr)); + f.write(reinterpret_cast(z.data()), + static_cast(z.size() * sizeof(float))); + const std::int32_t zoom = rgb.empty() ? 0 : imagery_zoom; + f.write(reinterpret_cast(&zoom), sizeof(zoom)); + if (zoom != 0) { + f.write(reinterpret_cast(rgb.data()), static_cast(rgb.size())); + } + if (!cell_valid.empty()) { + if (cell_valid.size() != static_cast(side - 1) * (side - 1)) {return false;} + f.write(kCellsTag, sizeof(kCellsTag)); + f.write(reinterpret_cast(cell_valid.data()), + static_cast(cell_valid.size())); + } + f.close(); + if (!f) {return false;} + std::filesystem::rename(tmp, path, ec); // atomic: readers never see a half-written file + return !ec; +} + +std::optional DemGrid::load(const std::string & path) +{ + std::ifstream f(path, std::ios::binary); + if (!f) {return std::nullopt;} + char magic[8]; + std::int32_t s = 0, syn = 0; + double hdr[7]; + f.read(magic, sizeof(magic)); + f.read(reinterpret_cast(&s), sizeof(s)); + f.read(reinterpret_cast(&syn), sizeof(syn)); + f.read(reinterpret_cast(hdr), sizeof(hdr)); + // side 1 is no grid (height_at reads node side - 2); the resolution and the header values are + // used as divisors and as the origin of the whole simulation + if (!f || std::memcmp(magic, kMagic, sizeof(kMagic)) != 0 || s < 3 || s % 2 == 0 || s > 20000 || + !(hdr[0] > 0.0) || + !std::isfinite(hdr[0]) || !std::isfinite(hdr[1]) || !std::isfinite(hdr[2]) || + !std::isfinite(hdr[3]) || !std::isfinite(hdr[4])) + { + return std::nullopt; + } + // a header must not make us allocate what the file does not hold: heights + zoom at least + const auto nodes = static_cast(s) * static_cast(s); + std::error_code size_ec; + const auto file_size = std::filesystem::file_size(path, size_ec); + const std::uintmax_t header_bytes = sizeof(magic) + 2 * sizeof(std::int32_t) + sizeof(hdr); + if (size_ec || file_size < header_bytes + nodes * sizeof(float) + sizeof(std::int32_t)) { + return std::nullopt; + } + DemGrid g; + g.side = s; + g.source = syn; + g.resolution = hdr[0]; + g.radius = hdr[1]; + g.origin_lat = hdr[2]; + g.origin_lon = hdr[3]; + g.reference_altitude = hdr[4]; + g.requested_reference = hdr[5]; + g.data_resolution = hdr[6]; + g.z.resize(static_cast(s) * s); + f.read(reinterpret_cast(g.z.data()), + static_cast(g.z.size() * sizeof(float))); + if (!f) {return std::nullopt;} + std::int32_t zoom = 0; + f.read(reinterpret_cast(&zoom), sizeof(zoom)); + if (!f) {return std::nullopt;} + if (zoom < 0 || zoom > 30) {return std::nullopt;} + if (std::any_of(g.z.begin(), g.z.end(), [](float v) {return !std::isfinite(v);})) { + return std::nullopt; + } + if (zoom != 0) { + if (file_size < header_bytes + nodes * (sizeof(float) + 3) + sizeof(std::int32_t)) { + return std::nullopt; + } + g.rgb.resize(g.z.size() * 3); + f.read(reinterpret_cast(g.rgb.data()), static_cast(g.rgb.size())); + if (!f) {return std::nullopt;} + g.imagery_zoom = zoom; + } + // the optional cells block: absent in older files (the file ends here); present, it is whole + char tag[8]; + f.read(tag, sizeof(tag)); + if (f.gcount() == 0) {return g;} + if (f.gcount() != sizeof(tag) || std::memcmp(tag, kCellsTag, sizeof(kCellsTag)) != 0) { + return std::nullopt; + } + g.cell_valid.resize(static_cast(s - 1) * (s - 1)); + f.read(reinterpret_cast(g.cell_valid.data()), + static_cast(g.cell_valid.size())); + if (!f) {return std::nullopt;} + for (auto & c : g.cell_valid) { + c = c != 0; + } + return g; +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/dynamic_layer.cpp b/maps_managers/easynav_bigmap_maps_manager/src/dynamic_layer.cpp new file mode 100644 index 00000000..51d7f878 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/dynamic_layer.cpp @@ -0,0 +1,382 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/dynamic_layer.hpp" + +#include +#include +#include + +namespace bigmap_maps_manager +{ + +TriangleLocator::TriangleLocator(const LodMesh & mesh, double cell) +: mesh_(&mesh), cell_(cell) +{ + double xmin = 1e300, ymin = 1e300, xmax = -1e300, ymax = -1e300; + for (std::size_t v = 0; v < mesh.x.size(); ++v) { + xmin = std::min(xmin, mesh.x[v]); xmax = std::max(xmax, mesh.x[v]); + ymin = std::min(ymin, mesh.y[v]); ymax = std::max(ymax, mesh.y[v]); + } + x0_ = xmin; + y0_ = ymin; + nx_ = std::max(1, static_cast(std::ceil((xmax - xmin) / cell_)) + 1); + ny_ = std::max(1, static_cast(std::ceil((ymax - ymin) / cell_)) + 1); + bins_.resize(static_cast(nx_) * ny_); + for (std::size_t t = 0; t < mesh.triangle_count(); ++t) { + const auto a = mesh.tri[3 * t], b = mesh.tri[3 * t + 1], c = mesh.tri[3 * t + 2]; + const double tx0 = std::min({mesh.x[a], mesh.x[b], mesh.x[c]}); + const double tx1 = std::max({mesh.x[a], mesh.x[b], mesh.x[c]}); + const double ty0 = std::min({mesh.y[a], mesh.y[b], mesh.y[c]}); + const double ty1 = std::max({mesh.y[a], mesh.y[b], mesh.y[c]}); + const int i0 = static_cast((tx0 - x0_) / cell_), + i1 = static_cast((tx1 - x0_) / cell_); + const int j0 = static_cast((ty0 - y0_) / cell_), + j1 = static_cast((ty1 - y0_) / cell_); + for (int j = j0; j <= std::min(j1, ny_ - 1); ++j) { + for (int i = i0; i <= std::min(i1, nx_ - 1); ++i) { + bins_[static_cast(j) * nx_ + i].push_back(static_cast(t)); + } + } + } +} + +int TriangleLocator::locate(double x, double y) const +{ + const int i = static_cast(std::floor((x - x0_) / cell_)); + const int j = static_cast(std::floor((y - y0_) / cell_)); + if (i < 0 || j < 0 || i >= nx_ || j >= ny_) {return -1;} + for (const auto t : bins_[static_cast(j) * nx_ + i]) { + const auto a = mesh_->tri[3 * t], b = mesh_->tri[3 * t + 1], c = mesh_->tri[3 * t + 2]; + const double x0 = mesh_->x[a], y0 = mesh_->y[a], x1 = mesh_->x[b], y1 = mesh_->y[b]; + const double x2 = mesh_->x[c], y2 = mesh_->y[c]; + const double det = (y1 - y2) * (x0 - x2) + (x2 - x1) * (y0 - y2); + if (std::abs(det) < 1e-12) {continue;} + const double l0 = ((y1 - y2) * (x - x2) + (x2 - x1) * (y - y2)) / det; + const double l1 = ((y2 - y0) * (x - x2) + (x0 - x2) * (y - y2)) / det; + if (l0 >= -1e-9 && l1 >= -1e-9 && 1.0 - l0 - l1 >= -1e-9) {return static_cast(t);} + } + return -1; +} + +namespace +{ +double plane_z(const LodMesh & m, std::uint32_t t, double x, double y); +} // namespace + +int TriangleLocator::locate_at(double x, double y, double z) const +{ + if (!std::isfinite(z)) {return locate(x, y);} + const int i = static_cast(std::floor((x - x0_) / cell_)); + const int j = static_cast(std::floor((y - y0_) / cell_)); + if (i < 0 || j < 0 || i >= nx_ || j >= ny_) {return -1;} + int below = -1, above = -1; + double below_z = -1e300, above_z = 1e300; + for (const auto t : bins_[static_cast(j) * nx_ + i]) { + const auto a = mesh_->tri[3 * t], b = mesh_->tri[3 * t + 1], c = mesh_->tri[3 * t + 2]; + const double x0 = mesh_->x[a], y0 = mesh_->y[a], x1 = mesh_->x[b], y1 = mesh_->y[b]; + const double x2 = mesh_->x[c], y2 = mesh_->y[c]; + const double det = (y1 - y2) * (x0 - x2) + (x2 - x1) * (y0 - y2); + if (std::abs(det) < 1e-12) {continue;} + const double l0 = ((y1 - y2) * (x - x2) + (x2 - x1) * (y - y2)) / det; + const double l1 = ((y2 - y0) * (x - x2) + (x0 - x2) * (y - y2)) / det; + if (!(l0 >= -1e-9 && l1 >= -1e-9 && 1.0 - l0 - l1 >= -1e-9)) {continue;} + const double pz = l0 * mesh_->z[a] + l1 * mesh_->z[b] + (1.0 - l0 - l1) * mesh_->z[c]; + if (pz <= z + 0.25) { + if (pz > below_z) {below_z = pz; below = static_cast(t);} + } else if (pz < above_z) { + above_z = pz; + above = static_cast(t); + } + } + return below >= 0 ? below : above; +} + +namespace +{ +// Height of the plane of triangle t at (x, y), and the z component of its unit normal +double plane_z(const LodMesh & m, std::uint32_t t, double x, double y) +{ + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const double det = (m.y[b] - m.y[c]) * (m.x[a] - m.x[c]) + (m.x[c] - m.x[b]) * (m.y[a] - m.y[c]); + if (std::abs(det) < 1e-12) {return m.z[a];} + const double l0 = ((m.y[b] - m.y[c]) * (x - m.x[c]) + (m.x[c] - m.x[b]) * (y - m.y[c])) / det; + const double l1 = ((m.y[c] - m.y[a]) * (x - m.x[c]) + (m.x[a] - m.x[c]) * (y - m.y[c])) / det; + return l0 * m.z[a] + l1 * m.z[b] + (1.0 - l0 - l1) * m.z[c]; +} + +double normal_z(const LodMesh & m, std::uint32_t t) +{ + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const double ux = m.x[b] - m.x[a], uy = m.y[b] - m.y[a], uz = m.z[b] - m.z[a]; + const double vx = m.x[c] - m.x[a], vy = m.y[c] - m.y[a], vz = m.z[c] - m.z[a]; + const double nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx; + const double len = std::sqrt(nx * nx + ny * ny + nz * nz); + return len < 1e-12 ? 1.0 : std::abs(nz) / len; +} +} // namespace + +bool DynamicLayer::ingest( + const std::vector> & points, double robot_x, double robot_y, + double robot_yaw, double now, const Surface & terrain, const DynamicParams & p, + const LodMesh * mesh, const TriangleLocator * locator) +{ + return ingest( + points, robot_x, robot_y, + std::array{0.0, 0.0, std::sin(robot_yaw / 2.0), std::cos(robot_yaw / 2.0)}, now, + terrain, p, mesh, locator); +} + +bool DynamicLayer::ingest( + const std::vector> & points, double robot_x, double robot_y, + const std::array & q_xyzw, double now, const Surface & terrain, + const DynamicParams & p, const LodMesh * mesh, const TriangleLocator * locator, + double robot_z) +{ + // rotation matrix of the robot's orientation (body -> map) + double qx = q_xyzw[0], qy = q_xyzw[1], qz = q_xyzw[2], qw = q_xyzw[3]; + const double qn = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw); + if (qn < 1e-9) {qx = qy = qz = 0.0; qw = 1.0;} else {qx /= qn; qy /= qn; qz /= qn; qw /= qn;} + const double r00 = 1 - 2 * (qy * qy + qz * qz), r01 = 2 * (qx * qy - qz * qw); + const double r02 = 2 * (qx * qz + qy * qw); + const double r10 = 2 * (qx * qy + qz * qw), r11 = 1 - 2 * (qx * qx + qz * qz); + const double r12 = 2 * (qy * qz - qx * qw); + const double r20 = 2 * (qx * qz - qy * qw), r21 = 2 * (qy * qz + qx * qw); + const double r22 = 1 - 2 * (qx * qx + qy * qy); + + const bool planes = mesh && locator; + double ground = terrain(robot_x, robot_y); // terrain under the robot: the heights' reference + if (planes) { + const int rt = locator->locate_at(robot_x, robot_y, robot_z); + if (rt >= 0) {ground = plane_z(*mesh, static_cast(rt), robot_x, robot_y);} + } + + // 1) Put every point in the map and group them by cell: an obstacle is told from ground that + // the terrain model puts too low by its vertical extent, which needs all the points. + struct Cell + { + double x, y, zmin, zmax; // map frame; x, y of the highest point + double bx, by, bz_min, bz_max, bx_lo, by_lo; // body frame (z of the lowest / highest point) + }; + std::unordered_map cells; + for (const auto & q : points) { + if (!std::isfinite(q[0]) || !std::isfinite(q[1]) || !std::isfinite(q[2])) {continue;} + if (std::hypot(q[0], q[1]) > p.max_range || q[2] > p.max_height) {continue;} + // the point in the map: the robot's ground contact plus the point rotated by its orientation + const double wx = robot_x + r00 * q[0] + r01 * q[1] + r02 * q[2]; + const double wy = robot_y + r10 * q[0] + r11 * q[1] + r12 * q[2]; + const double wz = ground + r20 * q[0] + r21 * q[1] + r22 * q[2]; + const std::int64_t key = + static_cast( // shifted as unsigned: negative indices are fine + static_cast(static_cast(std::floor(wx / p.cell))) << 32) ^ + (static_cast(std::floor(wy / p.cell)) & 0xffffffffLL); + auto it = cells.find(key); + if (it == cells.end()) { + cells[key] = {wx, wy, wz, wz, q[0], q[1], q[2], q[2], q[0], q[1]}; + } else { + Cell & cc = it->second; + if (wz > cc.zmax) {cc.zmax = wz; cc.x = wx; cc.y = wy; cc.bx = q[0]; cc.by = q[1];} + cc.zmin = std::min(cc.zmin, wz); + if (q[2] > cc.bz_max) {cc.bz_max = q[2];} + if (q[2] < cc.bz_min) {cc.bz_min = q[2]; cc.bx_lo = q[0]; cc.by_lo = q[1];} + } + } + + // 1b) Local ground plane in the window ahead, from the lowest point of each cell + // (z = a x + b y + c, robot frame). RANSAC over 3 cells, tilt limited; the best plane is + // refitted on its inliers. + struct Plane + { + double a{0}, b{0}, c{0}; + bool ok{false}; + } gp; + auto in_window = [&p](double bx, double by) { + return bx >= 0.0 && bx <= p.window_length && std::abs(by) <= 0.5 * p.window_width; + }; + if (p.ground_fit) { + std::vector> low; + for (const auto & kv : cells) { + const Cell & cc = kv.second; + if (in_window(cc.bx_lo, cc.by_lo) && cc.bz_min >= p.window_z_min && + cc.bz_min <= p.window_z_max) + { + low.push_back({cc.bx_lo, cc.by_lo, cc.bz_min}); + } + } + if (static_cast(low.size()) >= p.ground_min_cells) { + const double max_tilt = std::tan(p.max_ground_tilt_deg * M_PI / 180.0); + std::uint32_t seed = 12345u; // deterministic + auto rnd = [&seed, &low]() { + seed = seed * 1664525u + 1013904223u; + return static_cast(seed >> 8) % low.size(); + }; + std::size_t best = 0; + Plane bp; + for (int it = 0; it < 80; ++it) { + const auto & P0 = low[rnd()], & P1 = low[rnd()], & P2 = low[rnd()]; + const double ux = P1[0] - P0[0], uy = P1[1] - P0[1], uz = P1[2] - P0[2]; + const double vx = P2[0] - P0[0], vy = P2[1] - P0[1], vz = P2[2] - P0[2]; + const double nz = ux * vy - uy * vx; + if (std::abs(nz) < 1e-6) {continue;} + const double a = -(uy * vz - uz * vy) / nz, b = -(uz * vx - ux * vz) / nz; + if (std::hypot(a, b) > max_tilt) {continue;} + const double c0 = P0[2] - a * P0[0] - b * P0[1]; + std::size_t n = 0; + for (const auto & q : low) { + if (std::abs(q[2] - (a * q[0] + b * q[1] + c0)) <= p.ground_tolerance) {++n;} + } + if (n > best) {best = n; bp = {a, b, c0, true};} + } + if (bp.ok && static_cast(best) >= p.ground_min_cells) { + // least squares on the inliers (normal equations, 3x3) + double sxx = 0, sxy = 0, sx = 0, syy = 0, sy = 0, sn = 0, sxz = 0, syz = 0, sz = 0; + for (const auto & q : low) { + if (std::abs(q[2] - (bp.a * q[0] + bp.b * q[1] + bp.c)) > p.ground_tolerance) {continue;} + sxx += q[0] * q[0]; sxy += q[0] * q[1]; sx += q[0]; syy += q[1] * q[1]; sy += q[1]; + sn += 1; sxz += q[0] * q[2]; syz += q[1] * q[2]; sz += q[2]; + } + const double m[3][3] = {{sxx, sxy, sx}, {sxy, syy, sy}, {sx, sy, sn}}; + const double rhs[3] = {sxz, syz, sz}; + auto det3 = [](const double A[3][3]) { + return A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) - + A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) + + A[0][2] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); + }; + const double d = det3(m); + if (std::abs(d) > 1e-9) { + double sol[3]; + for (int k = 0; k < 3; ++k) { + double mk[3][3]; + for (int r = 0; r < 3; ++r) { + for (int cc = 0; cc < 3; ++cc) { + mk[r][cc] = (cc == k) ? rhs[r] : m[r][cc]; + } + } + sol[k] = det3(mk) / d; + } + if (std::hypot(sol[0], sol[1]) <= max_tilt) {bp = {sol[0], sol[1], sol[2], true};} + } + gp = bp; + } + } + } + + // 2) A cell is an obstacle if its highest point is at least min_height above the plane of the + // NavCel it falls in (along the normal) and its points span at least min_vertical_extent + bool changed = false; + for (const auto & kv : cells) { + const Cell & c = kv.second; + if (c.zmax - c.zmin < p.min_vertical_extent) {continue;} + // the sensor's own ground plane, where there is one: the highest point has to rise above it + // (along its normal) too, or the cell is just the slope the robot is looking at + if (gp.ok && in_window(c.bx, c.by)) { + const double norm = std::sqrt(1.0 + gp.a * gp.a + gp.b * gp.b); + const double above = (c.bz_max - (gp.a * c.bx + gp.b * c.by + gp.c)) / norm; + if (above < p.min_height) {continue;} + } + double surface, cos_n = 1.0; + if (planes) { + const int t = locator->locate_at(c.x, c.y, c.zmax); + if (t < 0) {continue;} + surface = plane_z(*mesh, static_cast(t), c.x, c.y); + cos_n = normal_z(*mesh, static_cast(t)); + } else { + surface = terrain(c.x, c.y); + } + // distance to the NavCel's plane along its normal = vertical offset * cos(slope) + if ((c.zmax - surface) * cos_n < p.min_height) {continue;} + auto it = entries_.find(kv.first); + if (it == entries_.end()) { + entries_[kv.first] = {static_cast(c.x), static_cast(c.y), + static_cast(c.zmax), static_cast(surface), now, now, p.confirm_time <= 0.0, + now, 0}; + changed = changed || p.confirm_time <= 0.0; + } else { + if (c.zmax > it->second.z + 0.05 && it->second.confirmed) {changed = true;} + it->second.z = std::max(it->second.z, static_cast(c.zmax)); + constexpr double kStreakGap = 1.0; // s: a longer break starts the run again + if (now - it->second.stamp > kStreakGap) { + it->second.streak = now; + it->second.periods = 0; + } + it->second.stamp = now; + } + } + return changed; +} + +bool DynamicLayer::expire(double now, const DynamicParams & p) +{ + // s: an unconfirmed detection that is gone was a glitch + constexpr double kUnconfirmedTimeout = 0.5; + bool changed = false; + for (auto it = entries_.begin(); it != entries_.end(); ) { + Entry & e = it->second; + const double unseen = now - e.stamp; + if (unseen > p.forget_time || (!e.confirmed && unseen > kUnconfirmedTimeout)) { + changed = changed || e.confirmed; + it = entries_.erase(it); + continue; + } + if (!e.confirmed && e.stamp - e.first >= p.confirm_time) { + e.confirmed = true; + changed = true; + } + ++it; + } + return changed; +} + +std::vector DynamicLayer::sustained(double now, double min_time) +{ + std::vector out; + if (min_time <= 0.0) {return out;} + for (auto & kv : entries_) { + Entry & e = kv.second; + if (!e.confirmed || now - e.stamp > 1.0) {continue;} // still being seen + const int due = static_cast((e.stamp - e.streak) / min_time); + if (due > e.periods) { + e.periods = due; + out.push_back({e.x, e.y, e.z, e.surface}); + } + } + return out; +} + +std::vector DynamicLayer::tiles( + const LodMesh & mesh, const TriangleLocator & locator, double min_age) const +{ + std::map top; + for (const auto & kv : entries_) { + if (!kv.second.confirmed) {continue;} + if (min_age > 0.0 && kv.second.stamp - kv.second.streak < min_age) {continue;} + const int t = locator.locate_at(kv.second.x, kv.second.y, kv.second.surface); + if (t < 0) {continue;} + // (a map's default float is 0, which would clip every height below the DEM's reference) + const auto it = top.emplace(static_cast(t), kv.second.z); + if (!it.second) {it.first->second = std::max(it.first->second, kv.second.z);} + } + std::vector out; + for (const auto & kv : top) { + const auto t = kv.first; + const auto a = mesh.tri[3 * t], b = mesh.tri[3 * t + 1], c = mesh.tri[3 * t + 2]; + const float highest = std::max({mesh.z[a], mesh.z[b], mesh.z[c]}); + const float mean = (mesh.z[a] + mesh.z[b] + mesh.z[c]) / 3.0f; + const float z_top = std::max(kv.second, highest); + out.push_back({t, z_top, z_top - mean}); + } + return out; +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/known_map.cpp b/maps_managers/easynav_bigmap_maps_manager/src/known_map.cpp new file mode 100644 index 00000000..1e6eaf60 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/known_map.cpp @@ -0,0 +1,191 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/known_map.hpp" + +#include +#include +#include + +namespace bigmap_maps_manager +{ + +KnownMap::KnownMap(int side, double resolution, int /*grid*/) +: side_(side), res_(resolution), center_((side - 1) / 2), + mark_(static_cast(side_) * side_, 0) +{ + // The marks cover the DEM's own nodes only: distance() answers for any node (the padding of the + // RTIN grid too) from the tree of the marked ones, so a mark per padded node (2.7 times the + // DEM's nodes for a side of 5001) bought nothing +} + +int KnownMap::mark(double x, double y, double radius) +{ + const double fx = x / res_ + center_, fy = y / res_ + center_; + const int reach = static_cast(std::ceil(radius / res_)); + const int cx = static_cast(std::lround(fx)), cy = static_cast(std::lround(fy)); + int added = 0; + for (int j = cy - reach; j <= cy + reach; ++j) { + for (int i = cx - reach; i <= cx + reach; ++i) { + if (i < 0 || j < 0 || i >= side_ || j >= side_) {continue;} + if (std::hypot((i - fx) * res_, (j - fy) * res_) > radius) {continue;} + auto & m = mark_[static_cast(j) * side_ + i]; + if (!m) { + m = 1; + nodes_.push_back(static_cast(static_cast(j) * side_ + i)); + ++added; + } + } + } + count_ += added; + dirty_ = dirty_ || added > 0; + return added; +} + +bool KnownMap::mark_node(int ix, int iy) +{ + if (ix < 0 || iy < 0 || ix >= side_ || iy >= side_) {return false;} + auto & m = mark_[static_cast(iy) * side_ + ix]; + if (m) {return false;} + m = 1; + nodes_.push_back(static_cast(static_cast(iy) * side_ + ix)); + ++count_; + dirty_ = true; + return true; +} + +void KnownMap::update() +{ + if (!dirty_) {return;} + dirty_ = false; + compute(nodes_); +} + +void KnownMap::update_within(double x, double y, double radius) +{ + const double fx = x / res_ + center_, fy = y / res_ + center_; + std::vector src; + for (const auto k : nodes_) { + const std::uint32_t side = static_cast(side_); + const double i = k % side, j = k / side; + if (std::hypot((i - fx) * res_, (j - fy) * res_) <= radius) {src.push_back(k);} + } + dirty_ = true; // the next update() starts from all of them again + compute(src); +} + +std::size_t KnownMap::forget_outside(double x, double y, double radius) +{ + const double fx = x / res_ + center_, fy = y / res_ + center_; + const std::uint32_t side = static_cast(side_); + std::vector kept; + kept.reserve(nodes_.size()); + std::size_t forgotten = 0; + for (const auto k : nodes_) { + const double i = k % side, j = k / side; + if (std::hypot((i - fx) * res_, (j - fy) * res_) <= radius) { + kept.push_back(k); + } else { + mark_[k] = 0; + ++forgotten; + } + } + if (forgotten > 0) { + nodes_ = std::move(kept); + count_ = nodes_.size(); + dirty_ = true; + } + return forgotten; +} + +bool KnownMap::near_marked(int ix, int iy, int reach) const +{ + for (int j = iy - reach; j <= iy + reach; ++j) { + for (int i = ix - reach; i <= ix + reach; ++i) { + if (known(i, j)) {return true;} + } + } + return false; +} + +// A balanced 2-D tree of the source nodes, built in place over `tree_` (the median along the +// wider axis of each half, recursively): the exact nearest source of any node is found in +// O(log n) on average, so a re-mesh costs the few tens of thousands of queries the mesher makes +// instead of an exact distance transform of the whole grid (60 ms of a 75 ms re-mesh at 4 M +// nodes; the mesher visits a few hundred nodes per metre of path, not the grid) +void KnownMap::compute(const std::vector & sources) +{ + sources_ = sources.size(); + tree_.clear(); + tree_.reserve(sources.size()); + const auto side = static_cast(side_); + for (const auto k : sources) { + tree_.push_back({static_cast(k % side), static_cast(k / side)}); + } + build(0, tree_.size()); +} + +void KnownMap::build(std::size_t lo, std::size_t hi) +{ + // each node of the tree is the median of its range: [lo, mid) left, (mid, hi) right + if (hi - lo <= 1) {return;} + int min_x = tree_[lo].x, max_x = min_x, min_y = tree_[lo].y, max_y = min_y; + for (std::size_t i = lo + 1; i < hi; ++i) { + min_x = std::min(min_x, tree_[i].x); + max_x = std::max(max_x, tree_[i].x); + min_y = std::min(min_y, tree_[i].y); + max_y = std::max(max_y, tree_[i].y); + } + const bool by_x = max_x - min_x >= max_y - min_y; + const std::size_t mid = lo + (hi - lo) / 2; + std::nth_element( + tree_.begin() + lo, tree_.begin() + mid, tree_.begin() + hi, + [by_x](const Node & a, const Node & b) {return by_x ? a.x < b.x : a.y < b.y;}); + tree_[mid].by_x = by_x; + build(lo, mid); + build(mid + 1, hi); +} + +void KnownMap::nearest(std::size_t lo, std::size_t hi, int x, int y, double & best) const +{ + if (hi <= lo) {return;} + const std::size_t mid = lo + (hi - lo) / 2; + const Node & n = tree_[mid]; + const double dx = static_cast(x - n.x), dy = static_cast(y - n.y); + best = std::min(best, dx * dx + dy * dy); + const double delta = n.by_x ? dx : dy; // signed distance to the splitting line + if (delta < 0.0) { + nearest(lo, mid, x, y, best); + if (delta * delta < best) {nearest(mid + 1, hi, x, y, best);} + } else { + nearest(mid + 1, hi, x, y, best); + if (delta * delta < best) {nearest(lo, mid, x, y, best);} + } +} + +double KnownMap::distance(int ix, int iy) const +{ + if (tree_.empty()) {return std::numeric_limits::infinity();} + double best = std::numeric_limits::infinity(); + nearest(0, tree_.size(), ix, iy, best); + return std::sqrt(best) * res_; +} + +bool KnownMap::known(int ix, int iy) const +{ + if (ix < 0 || iy < 0 || ix >= side_ || iy >= side_) {return false;} + return mark_[static_cast(iy) * side_ + ix] != 0; +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/lod_mesher.cpp b/maps_managers/easynav_bigmap_maps_manager/src/lod_mesher.cpp new file mode 100644 index 00000000..4b0bf8be --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/lod_mesher.cpp @@ -0,0 +1,186 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" + +#include +#include +#include +#include +#include + +namespace bigmap_maps_manager +{ + +namespace +{ +struct Extractor +{ + const RtinErrorMap & em; + const DemGrid & dem; + const LodParams & p; + LodParams po; // the policy for the observed ground + double rx, ry; + const KnownMap * known; + const KnownMap * observed; + const std::vector * valid; // per node; empty: all valid + const std::vector * cell_valid; // per cell; empty: the node rule + LodMesh & out; + std::unordered_map & index; // grid node -> output vertex + int center; // index of the DEM origin node + int last; // last real node index + + uint32_t vertex(int x, int y) + { + const auto key = static_cast(y) * static_cast(em.grid_size()) + + static_cast(x); + const auto it = index.emplace(key, static_cast(out.x.size())); + if (it.second) { + out.x.push_back(static_cast((x - center) * dem.resolution)); + out.y.push_back(static_cast((y - center) * dem.resolution)); + out.z.push_back(em.height(x, y)); + if (!dem.rgb.empty()) { + // padding nodes (beyond the real DEM) replicate the edge colour + const std::size_t kk = (static_cast(std::min(y, last)) * dem.side + + std::min(x, last)) * 3; + out.r.push_back(dem.rgb[kk]); + out.g.push_back(dem.rgb[kk + 1]); + out.b.push_back(dem.rgb[kk + 2]); + } + } + return it.first->second; + } + + // the smaller of the robot's effective distance and those of the traversed / observed ground + double distance(int mx, int my, double hyp) const + { + const double dist = std::hypot((mx - center) * dem.resolution - rx, + (my - center) * dem.resolution - ry); + double d_eff = effective_distance(dist, hyp); + // each set keeps full resolution within its margin of it, wherever the robot is; the finer + // distance decides (the minimum keeps the crack-free order, see lod_policy.hpp) + if (known && !known->empty()) { + d_eff = std::min(d_eff, known_effective_distance(p, known->distance(mx, my), hyp)); + } + if (observed && !observed->empty()) { + d_eff = std::min(d_eff, known_effective_distance(po, observed->distance(mx, my), hyp)); + } + return d_eff; + } + + bool emitted(int ax, int ay, int bx, int by, int cx, int cy) const + { + if (!cell_valid->empty()) { + // the cell under the centroid (exact for a NavCel of the grid's resolution: it is inside + // one cell); a cell of the padding is no data + const double gx = (ax + bx + cx) / 3.0, gy = (ay + by + cy) / 3.0; + const int i = static_cast(std::floor(gx)), j = static_cast(std::floor(gy)); + if (i < 0 || j < 0 || i >= last || j >= last) {return false;} + return (*cell_valid)[static_cast(j) * last + i] != 0; + } + if (!valid->empty()) { + // a NavCel centred on no-data ground does not exist + const int gx = (ax + bx + cx + 1) / 3, gy = (ay + by + cy + 1) / 3; + return gx <= last && gy <= last && (*valid)[static_cast(gy) * dem.side + gx]; + } + return true; + } + + void process(int ax, int ay, int bx, int by, int cx, int cy) + { + // Whole subtree lies in the padding: nothing real to show (children stay inside the bbox). + if (std::min({ax, bx, cx}) >= last || std::min({ay, by, cy}) >= last) { + return; + } + const int mx = (ax + bx) >> 1; + const int my = (ay + by) >> 1; + const bool splittable = std::abs(ax - cx) + std::abs(ay - cy) > 1; + if (splittable) { + const double leg = std::hypot(ax - cx, ay - cy) * dem.resolution; + const double hyp = leg * M_SQRT2; + const double d_eff = distance(mx, my, hyp); + // a narrow passage is split down to the grid's resolution whatever the distance + if (em.pinned(mx, my) || should_split_at(p, em.error(mx, my), leg, d_eff)) { + process(cx, cy, ax, ay, mx, my); + process(bx, by, cx, cy, mx, my); + return; + } + } + if (!emitted(ax, ay, bx, by, cx, cy)) {return;} + // Source winding is clockwise (y up); emit CCW. + const uint32_t va = vertex(ax, ay), vb = vertex(bx, by), vc = vertex(cx, cy); + out.tri.insert(out.tri.end(), {va, vc, vb}); + out.leg.push_back(static_cast(std::hypot(ax - cx, ay - cy) * dem.resolution)); + } +}; +} // namespace + +std::vector hole_border_nodes(int side, const std::vector & cell_valid) +{ + const int nc = side - 1; + if (nc < 1 || cell_valid.size() != static_cast(nc) * nc) {return {};} + std::vector border(static_cast(side) * side, 0); + for (int y = 0; y < side; ++y) { + for (int x = 0; x < side; ++x) { + bool some = false, none = false; + for (int j = y - 1; j <= y; ++j) { + for (int i = x - 1; i <= x; ++i) { + if (i < 0 || j < 0 || i >= nc || j >= nc) {continue;} + (cell_valid[static_cast(j) * nc + i] ? some : none) = true; + } + } + border[static_cast(y) * side + x] = some && none; + } + } + return border; +} + +LodMesher::LodMesher( + DemGrid dem, std::vector valid, + const std::vector & pinned, std::vector cell_valid, + double hole_error) +: dem_(std::move(dem)), + errors_(dem_, pinned, + hole_error > 0.0 ? hole_border_nodes(dem_.side, cell_valid) : std::vector{}, + hole_error), + valid_(std::move(valid)) +{ + const std::size_t nc = static_cast(std::max(0, dem.side - 1)); + if (cell_valid.size() == nc * nc) {cell_valid_ = std::move(cell_valid);} +} + +LodMesh LodMesher::extract( + double robot_x, double robot_y, const LodParams & params, const KnownMap * known, + const KnownMap * observed) const +{ + LodMesh mesh; + std::unordered_map index; + index.reserve(4096); + const int center = (dem_.side - 1) / 2; + Extractor ex{errors_, dem_, params, observed_params(params), robot_x, robot_y, known, observed, + &valid_, &cell_valid_, + mesh, index, center, dem_.side - 1}; + const int ts = errors_.tile_size(); + ex.process(0, 0, ts, ts, ts, 0); + ex.process(ts, ts, 0, 0, 0, ts); + return mesh; +} + +std::size_t LodMesher::memory_bytes() const +{ + return dem_.z.capacity() * sizeof(float) + dem_.rgb.capacity() + errors_.memory_bytes() + + valid_.capacity() + cell_valid_.capacity(); +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/lod_policy.cpp b/maps_managers/easynav_bigmap_maps_manager/src/lod_policy.cpp new file mode 100644 index 00000000..6b62df0c --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/lod_policy.cpp @@ -0,0 +1,17 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Header-only policy; this translation unit keeps the library target non-empty and checks +// at compile time that the header is self-contained. +#include "easynav_bigmap_maps_manager/lod_policy.hpp" diff --git a/maps_managers/easynav_bigmap_maps_manager/src/marker_publisher.cpp b/maps_managers/easynav_bigmap_maps_manager/src/marker_publisher.cpp new file mode 100644 index 00000000..bdfa9cc9 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/marker_publisher.cpp @@ -0,0 +1,130 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/marker_publisher.hpp" + +#include +#include + +#include "geometry_msgs/msg/point.hpp" +#include "std_msgs/msg/color_rgba.hpp" + +namespace bigmap_maps_manager +{ + +namespace +{ +std_msgs::msg::ColorRGBA lod_color(float leg, float min_leg, float max_leg) +{ + // log scale: each doubling of the triangle size moves the same amount along green -> red + const float t = std::clamp( + std::log2(leg / min_leg) / std::max(1e-3f, std::log2(max_leg / min_leg)), 0.0f, 1.0f); + std_msgs::msg::ColorRGBA c; + c.r = t; + c.g = 1.0f - 0.6f * t; + c.b = 0.15f; + c.a = 1.0f; + return c; +} + +geometry_msgs::msg::Point pt(float x, float y, float z) +{ + geometry_msgs::msg::Point p; + p.x = x; p.y = y; p.z = z; + return p; +} +} // namespace + +visualization_msgs::msg::MarkerArray make_markers( + const LodMesh & mesh, const LodParams & params, double robot_x, double robot_y, double robot_z, + double max_leg, const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + double outer_radius) +{ + using visualization_msgs::msg::Marker; + const float min_leg = static_cast(std::max(1e-3, params.min_size)); + const std::size_t nt = mesh.triangle_count(); + + Marker tris; + tris.header.frame_id = frame_id; + tris.header.stamp = stamp; + tris.ns = "lod_triangles"; + tris.id = 0; + tris.type = Marker::TRIANGLE_LIST; + tris.action = Marker::ADD; + tris.pose.orientation.w = 1.0; + tris.scale.x = tris.scale.y = tris.scale.z = 1.0; + tris.color.a = 1.0; + tris.points.reserve(nt * 3); + tris.colors.reserve(nt * 3); + + Marker wire = tris; + wire.ns = "lod_wireframe"; + wire.type = Marker::LINE_LIST; + wire.scale.x = 0.05; + wire.color.r = wire.color.g = wire.color.b = 0.05f; + wire.colors.clear(); + wire.points.clear(); + wire.points.reserve(nt * 6); + + for (std::size_t t = 0; t < nt; ++t) { + const auto col = lod_color(mesh.leg[t], min_leg, static_cast(max_leg)); + uint32_t v[3] = {mesh.tri[3 * t], mesh.tri[3 * t + 1], mesh.tri[3 * t + 2]}; + for (int k = 0; k < 3; ++k) { + tris.points.push_back(pt(mesh.x[v[k]], mesh.y[v[k]], mesh.z[v[k]])); + tris.colors.push_back(col); + const uint32_t a = v[k], b = v[(k + 1) % 3]; + wire.points.push_back(pt(mesh.x[a], mesh.y[a], mesh.z[a] + 0.05f)); + wire.points.push_back(pt(mesh.x[b], mesh.y[b], mesh.z[b] + 0.05f)); + } + } + + Marker ring; + ring.header = tris.header; + ring.ns = "lod_high_res_radius"; + ring.id = 0; + ring.type = Marker::LINE_STRIP; + ring.action = Marker::ADD; + ring.pose.orientation.w = 1.0; + ring.scale.x = 0.25; + ring.color.r = 0.1f; ring.color.g = 0.4f; ring.color.b = 1.0f; ring.color.a = 1.0f; + constexpr int kSegments = 96; + for (int i = 0; i <= kSegments; ++i) { + const double a = 2.0 * M_PI * i / kSegments; + ring.points.push_back(pt( + static_cast(robot_x + params.high_res_radius * std::cos(a)), + static_cast(robot_y + params.high_res_radius * std::sin(a)), + static_cast(robot_z + 0.5))); + } + + visualization_msgs::msg::MarkerArray arr; + arr.markers = {tris, wire, ring}; + if (outer_radius > 0.0) { + // the larger circle: inside it, ground the robot has driven over keeps full resolution + Marker outer = ring; + outer.ns = "lod_traversed_radius"; + outer.color.r = 1.0f; outer.color.g = 0.6f; outer.color.b = 0.1f; + outer.points.clear(); + for (int i = 0; i <= kSegments; ++i) { + const double a = 2.0 * M_PI * i / kSegments; + outer.points.push_back(pt( + static_cast(robot_x + outer_radius * std::cos(a)), + static_cast(robot_y + outer_radius * std::sin(a)), + static_cast(robot_z + 0.5))); + } + arr.markers.push_back(outer); + } + return arr; +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/narrow_passages.cpp b/maps_managers/easynav_bigmap_maps_manager/src/narrow_passages.cpp new file mode 100644 index 00000000..6baa985e --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/narrow_passages.cpp @@ -0,0 +1,177 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/narrow_passages.hpp" + +#include +#include + +namespace bigmap_maps_manager +{ + +std::vector drivable_nodes( + const DemGrid & dem, const std::vector & valid, double max_slope_deg) +{ + const int n = dem.side; + const auto at = [n](int x, int y) {return static_cast(y) * n + x;}; + const double max_dz = std::tan(max_slope_deg * M_PI / 180.0) * dem.resolution; + std::vector ok(static_cast(n) * n, 0); + for (int y = 0; y < n; ++y) { + for (int x = 0; x < n; ++x) { + if (!valid.empty() && !valid[at(x, y)]) {continue;} + const float z = dem.z[at(x, y)]; + bool flat = true; + for (const auto & [dx, dy] : {std::pair{1, 0}, {-1, 0}, {0, 1}, {0, -1}}) { + const int u = x + dx, v = y + dy; + if (u < 0 || v < 0 || u >= n || v >= n) {continue;} + if (!valid.empty() && !valid[at(u, v)]) {continue;} + if (z - dem.z[at(u, v)] > max_dz) {flat = false; break;} + } + ok[at(x, y)] = flat; + } + } + return ok; +} + +std::vector keep_main_piece( + const DemGrid & dem, const std::vector & valid, double max_slope_deg, + std::size_t * dropped) +{ + const int n = dem.side; + const std::size_t total = static_cast(n) * n; + const auto ok = drivable_nodes(dem, valid, max_slope_deg); + // pieces of drivable nodes (4-connected), by flood fill + std::vector piece(total, -1); + std::vector sizes, stack; + for (std::size_t seed = 0; seed < total; ++seed) { + if (!ok[seed] || piece[seed] >= 0) {continue;} + const int id = static_cast(sizes.size()); + sizes.push_back(0); + stack.assign(1, seed); + piece[seed] = id; + while (!stack.empty()) { + const std::size_t k = stack.back(); + stack.pop_back(); + ++sizes[id]; + const int x = static_cast(k % n), y = static_cast(k / n); + for (const auto & [dx, dy] : {std::pair{1, 0}, {-1, 0}, {0, 1}, {0, -1}}) { + const int u = x + dx, v = y + dy; + if (u < 0 || v < 0 || u >= n || v >= n) {continue;} + const std::size_t q = static_cast(v) * n + u; + if (ok[q] && piece[q] < 0) { + piece[q] = id; + stack.push_back(q); + } + } + } + } + std::vector out(total, 0); + if (sizes.empty()) { + if (dropped) {*dropped = 0;} + return valid.empty() ? std::vector(total, 1) : valid; // nothing drivable + } + const int main = static_cast( + std::max_element(sizes.begin(), sizes.end()) - sizes.begin()); + std::size_t lost = 0; + for (int y = 0; y < n; ++y) { + for (int x = 0; x < n; ++x) { + const std::size_t k = static_cast(y) * n + x; + const bool had = valid.empty() || valid[k]; + bool keep = false; + if (had) { + if (ok[k]) { + keep = piece[k] == main; + } else { // steep: kept if it touches the main piece (8-neighbourhood) + for (int dy = -1; dy <= 1 && !keep; ++dy) { + for (int dx = -1; dx <= 1 && !keep; ++dx) { + const int u = x + dx, v = y + dy; + if (u < 0 || v < 0 || u >= n || v >= n) {continue;} + keep = piece[static_cast(v) * n + u] == main; + } + } + } + } + out[k] = keep; + lost += had && !keep; + } + } + if (dropped) {*dropped = lost;} + return out; +} + +std::vector narrow_passage_nodes( + const DemGrid & dem, const std::vector & valid, + double max_slope_deg, double width) +{ + if (width <= 0.0 || dem.side < 3) {return {};} + const int n = dem.side; + const auto at = [n](int x, int y) {return static_cast(y) * n + x;}; + // drivable: data present and not on the face of a step or of a slope: the floor at the foot + // of a wall stays drivable + const auto ok = drivable_nodes(dem, valid, max_slope_deg); + + // Distance of every drivable node to the nearest undrivable one (the grid's own edge is not a + // border: the map goes on); the passage half-width around a node is the largest such distance + // in its surroundings, and the node is narrow when that is below half the width. + const double half = 0.5 * width; + const double reach = half + 0.5 * dem.resolution; + const int rr = static_cast(std::ceil(reach / dem.resolution)); + const auto far = static_cast(reach + dem.resolution); + std::vector to_border(ok.size(), 0.0f); + for (int y = 0; y < n; ++y) { + for (int x = 0; x < n; ++x) { + if (!ok[at(x, y)]) {continue;} + float best = far; + for (int j = std::max(0, y - rr); j <= std::min(n - 1, y + rr); ++j) { + for (int i = std::max(0, x - rr); i <= std::min(n - 1, x + rr); ++i) { + if (ok[at(i, j)]) {continue;} + const auto d = static_cast(std::hypot(i - x, j - y) * dem.resolution); + best = std::min(best, d); + } + } + to_border[at(x, y)] = best; + } + } + std::vector narrow(ok.size(), 0); + std::vector core(ok.size(), 0); + const int look = static_cast(std::ceil((half + 0.25 * dem.resolution) / dem.resolution)); + for (int y = 0; y < n; ++y) { + for (int x = 0; x < n; ++x) { + if (!ok[at(x, y)] || to_border[at(x, y)] >= far) {continue;} // open ground + float widest = 0.0f; + for (int j = std::max(0, y - look); j <= std::min(n - 1, y + look); ++j) { + for (int i = std::max(0, x - look); i <= std::min(n - 1, x + look); ++i) { + if (std::hypot(i - x, j - y) * dem.resolution > look * dem.resolution) {continue;} + widest = std::max(widest, to_border[at(i, j)]); + } + } + core[at(x, y)] = widest < half; + } + } + // the undrivable nodes next to a narrow passage (its walls) come with it, so its sides are as + // fine as its floor; walls and slopes with no drivable ground nearby are left alone + for (int y = 0; y < n; ++y) { + for (int x = 0; x < n; ++x) { + if (!core[at(x, y)]) {continue;} + for (int j = std::max(0, y - 1); j <= std::min(n - 1, y + 1); ++j) { + for (int i = std::max(0, x - 1); i <= std::min(n - 1, x + 1); ++i) { + narrow[at(i, j)] = 1; + } + } + } + } + return narrow; +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/navmap_dem.cpp b/maps_managers/easynav_bigmap_maps_manager/src/navmap_dem.cpp new file mode 100644 index 00000000..8a46a718 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/navmap_dem.cpp @@ -0,0 +1,386 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/navmap_dem.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/Costs.hpp" + +namespace bigmap_maps_manager +{ + +void SourceMesh::validate() const +{ + const std::size_t nv = x.size(), nt = v0.size(); + if (y.size() != nv || z.size() != nv || v1.size() != nt || v2.size() != nt) { + throw std::invalid_argument("the NavMap's arrays differ in size"); + } + for (std::size_t v = 0; v < nv; ++v) { + // a NaN would reach every mesh, or an index cast (undefined): refused, by name + if (!std::isfinite(x[v]) || !std::isfinite(y[v]) || !std::isfinite(z[v])) { + throw std::invalid_argument( + "the NavMap has a vertex with a non-finite position (vertex " + std::to_string(v) + + ")"); + } + } + for (std::size_t t = 0; t < nt; ++t) { + if (v0[t] >= nv || v1[t] >= nv || v2[t] >= nv) { + throw std::invalid_argument( + "the NavMap has a NavCel with a vertex index out of range (NavCel " + + std::to_string(t) + ")"); + } + } + if ((!traversed.empty() && traversed.size() != nt) || + (!obstacle_hits.empty() && obstacle_hits.size() != nt)) + { + throw std::invalid_argument("the NavMap has a layer of another size than its NavCels"); + } +} + +SourceMesh SourceMesh::from_navmap(const ::navmap::NavMap & map) +{ + SourceMesh m; + m.x = map.positions.x; + m.y = map.positions.y; + m.z = map.positions.z; + const std::size_t nt = map.navcels.size(); + m.v0.resize(nt); + m.v1.resize(nt); + m.v2.resize(nt); + for (std::size_t t = 0; t < nt; ++t) { + m.v0[t] = map.navcels[t].v[0]; + m.v1[t] = map.navcels[t].v[1]; + m.v2[t] = map.navcels[t].v[2]; + } + if (map.colors && map.colors->r.size() == m.x.size() && map.colors->g.size() == m.x.size() && + map.colors->b.size() == m.x.size()) + { + m.r = map.colors->r; + m.g = map.colors->g; + m.b = map.colors->b; + } + for (const char * name : {"traversed", "obstacle_hits"}) { + if (!map.layers.get(name)) {continue;} + auto & out = std::string(name) == "traversed" ? m.traversed : m.obstacle_hits; + out.resize(nt); + for (std::size_t t = 0; t < nt; ++t) { + out[t] = map.layer_get(name, static_cast<::navmap::NavCelId>(t), 0); + } + } + m.has_traversed_layer = map.layers.get("traversed") != nullptr; + return m; +} + +NavMapDem dem_from_navmap(const ::navmap::NavMap & map, double resolution, int max_side) +{ + if (map.navcels.empty() || map.positions.x.empty()) { + throw std::invalid_argument("the NavMap has no NavCels"); + } + return dem_from_navmap(SourceMesh::from_navmap(map), resolution, max_side); +} + +NavMapDem dem_from_navmap(const SourceMesh & map, double resolution, int max_side) +{ + if (map.triangle_count() == 0 || map.x.empty()) { + throw std::invalid_argument("the NavMap has no NavCels"); + } + if (resolution <= 0.0) {throw std::invalid_argument("resolution must be > 0");} + + const auto & px = map.x; + const auto & py = map.y; + const auto & pz = map.z; + const std::size_t nt = map.triangle_count(); + auto vtx = [&map](std::size_t t, int k) { + return k == 0 ? map.v0[t] : (k == 1 ? map.v1[t] : map.v2[t]); + }; + double half = 0.0; + for (std::size_t t = 0; t < nt; ++t) { + for (int k = 0; k < 3; ++k) { + const auto v = vtx(t, k); + if (v >= px.size()) {continue;} + // (a non-finite vertex is skipped below with its NavCel: it must not size the grid) + if (!std::isfinite(px[v]) || !std::isfinite(py[v])) {continue;} + half = std::max({half, std::abs(static_cast(px[v])), + std::abs(static_cast(py[v]))}); + } + } + // the grid size is checked BEFORE any cast to int: an absurd extent (a corrupt file) would + // overflow the cast (undefined) and then every index below + const double reach_d = std::ceil(half / resolution) + 1.0; + const std::int64_t side_l = 2 * static_cast(std::min(reach_d, 1e9)) + 1; + if (side_l > max_side) { + throw std::invalid_argument( + "the NavMap spans too much for a " + std::to_string(resolution) + " m grid (" + + std::to_string(side_l) + " nodes per side, at most " + std::to_string(max_side) + + "): raise lod_settings.min_triangle_size / initial_navmap_resolution"); + } + const int side = static_cast(side_l); + const int reach = (side - 1) / 2; + + NavMapDem out; + auto & dem = out.dem; + dem.side = side; + dem.resolution = resolution; + dem.data_resolution = resolution; + dem.radius = reach * resolution; + dem.source = DemGrid::kMesh; + const std::size_t n = static_cast(side) * side; + const int nc = side - 1; + constexpr float kNone = -std::numeric_limits::infinity(); + dem.z.assign(n, kNone); + out.valid.assign(n, 0); + out.cell_valid.assign(static_cast(nc) * nc, 0); + const bool coloured = map.coloured(); + if (coloured) {dem.rgb.assign(n * 3, 128);} + + const bool has_traversed = map.has_traversed_layer || !map.traversed.empty(); + out.has_traversed = has_traversed; + + for (std::size_t t = 0; t < nt; ++t) { + const auto a = vtx(t, 0), b = vtx(t, 1), c = vtx(t, 2); + if (a >= px.size() || b >= px.size() || c >= px.size()) {continue;} + const double x0 = px[a], y0 = py[a], x1 = px[b], y1 = py[b]; + const double x2 = px[c], y2 = py[c]; + if (!std::isfinite(x0) || !std::isfinite(y0) || !std::isfinite(x1) || !std::isfinite(y1) || + !std::isfinite(x2) || !std::isfinite(y2)) + { + continue; // (NaN passes every comparison below, and its cast to int is undefined) + } + const double det = (y1 - y2) * (x0 - x2) + (x2 - x1) * (y0 - y2); + if (std::abs(det) < 1e-12) {continue;} + const bool trav = !map.traversed.empty() && map.traversed[t] > 0; + const double xmin = std::min({x0, x1, x2}), xmax = std::max({x0, x1, x2}); + const double ymin = std::min({y0, y1, y2}), ymax = std::max({y0, y1, y2}); + const int i0 = std::max(0, static_cast(std::floor(xmin / resolution)) + reach); + const int i1 = std::min(side - 1, static_cast(std::ceil(xmax / resolution)) + reach); + const int j0 = std::max(0, static_cast(std::floor(ymin / resolution)) + reach); + const int j1 = std::min(side - 1, static_cast(std::ceil(ymax / resolution)) + reach); + auto bary = [&](double x, double y, double & l0, double & l1, double & l2) { + l0 = ((y1 - y2) * (x - x2) + (x2 - x1) * (y - y2)) / det; + l1 = ((y2 - y0) * (x - x2) + (x0 - x2) * (y - y2)) / det; + l2 = 1.0 - l0 - l1; + constexpr double kEps = -1e-9; + return l0 >= kEps && l1 >= kEps && l2 >= kEps; + }; + for (int j = j0; j <= j1; ++j) { + for (int i = i0; i <= i1; ++i) { + double l0, l1, l2; + // the cell whose lower corner is this node: covered if its centre is in the NavCel + if (i < nc && j < nc && + bary((i - reach + 0.5) * resolution, (j - reach + 0.5) * resolution, l0, l1, l2)) + { + out.cell_valid[static_cast(j) * nc + i] = 1; + } + const double x = (i - reach) * resolution, y = (j - reach) * resolution; + if (!bary(x, y, l0, l1, l2)) {continue;} + const std::size_t k = static_cast(j) * side + i; + const float z = static_cast(l0 * pz[a] + l1 * pz[b] + l2 * pz[c]); + if (!std::isfinite(z)) {continue;} // a NaN height is no data, not a node at -inf + if (z > dem.z[k]) { + dem.z[k] = z; + if (coloured) { + dem.rgb[3 * k] = static_cast( + l0 * map.r[a] + l1 * map.r[b] + l2 * map.r[c]); + dem.rgb[3 * k + 1] = static_cast( + l0 * map.g[a] + l1 * map.g[b] + l2 * map.g[c]); + dem.rgb[3 * k + 2] = static_cast( + l0 * map.b[a] + l1 * map.b[b] + l2 * map.b[c]); + } + } + if (!out.valid[k]) { + out.valid[k] = 1; + ++out.covered_nodes; + } + if (trav) {out.traversed_nodes.emplace_back(i, j);} + } + } + } + if (out.covered_nodes == 0) { + throw std::invalid_argument("no NavCel of the map contains a node of the grid"); + } + + // Close the sampling gaps (NavCels smaller than the grid can miss a node): two passes of "a + // node with at least 5 of its 8 neighbours covered takes their mean". 5, not fewer: a node just + // outside a straight border has 3, so the borders do not grow; a missing node inside the surface + // has 8 and a missing line of nodes 6, so they are filled. A gap of 2 nodes or more stays. + for (int pass = 0; pass < 2; ++pass) { + std::vector newly; + std::vector idx; + std::vector zs; + std::vector> cols; + for (int j = 1; j < side - 1; ++j) { + for (int i = 1; i < side - 1; ++i) { + const std::size_t k = static_cast(j) * side + i; + if (out.valid[k]) {continue;} + double sz = 0; + int cnt = 0; + std::array sc{0, 0, 0}; + for (int dj = -1; dj <= 1; ++dj) { + for (int di = -1; di <= 1; ++di) { + const std::size_t q = static_cast(j + dj) * side + (i + di); + if (!out.valid[q]) {continue;} + sz += dem.z[q]; + ++cnt; + if (coloured) { + for (int c = 0; c < 3; ++c) { + sc[c] += dem.rgb[3 * q + c]; + } + } + } + } + if (cnt >= 5) { + idx.push_back(k); + zs.push_back(static_cast(sz / cnt)); + cols.push_back({sc[0] / cnt, sc[1] / cnt, sc[2] / cnt}); + } + } + } + for (std::size_t m = 0; m < idx.size(); ++m) { + dem.z[idx[m]] = zs[m]; + out.valid[idx[m]] = 1; + ++out.covered_nodes; + if (coloured) { + for (int c = 0; c < 3; ++c) { + dem.rgb[3 * idx[m] + c] = cols[m][c]; + } + } + } + } + + // No-data nodes take the height of the nearest covered one (rings of dilation), so the error + // map and the interpolation never see an infinity; the mesher drops what is not valid + // (the frontier is the set of undone nodes next to a done one: each ring costs its own size, not + // the whole grid's) + std::vector done = out.valid; + std::vector queued(n, 0); + std::vector frontier; + auto push_neighbours = [&](int i, int j) { + for (int dj = -1; dj <= 1; ++dj) { + for (int di = -1; di <= 1; ++di) { + const int a = i + di, b = j + dj; + if (a < 0 || b < 0 || a >= side || b >= side) {continue;} + const std::size_t q = static_cast(b) * side + a; + if (!done[q] && !queued[q]) { + queued[q] = 1; + frontier.push_back(q); + } + } + } + }; + for (int j = 0; j < side; ++j) { + for (int i = 0; i < side; ++i) { + if (done[static_cast(j) * side + i]) {push_neighbours(i, j);} + } + } + while (!frontier.empty()) { + std::vector zs(frontier.size()); + for (std::size_t m = 0; m < frontier.size(); ++m) { + const int i = static_cast(frontier[m] % side), j = static_cast(frontier[m] / side); + double sz = 0; + int cnt = 0; + for (int dj = -1; dj <= 1; ++dj) { + for (int di = -1; di <= 1; ++di) { + const int a = i + di, b = j + dj; + if (a < 0 || b < 0 || a >= side || b >= side) {continue;} + const std::size_t q = static_cast(b) * side + a; + if (done[q]) {sz += dem.z[q]; ++cnt;} + } + } + zs[m] = static_cast(sz / cnt); // the frontier has at least one done neighbour + } + const std::vector ring = std::move(frontier); + frontier.clear(); + for (std::size_t m = 0; m < ring.size(); ++m) { + dem.z[ring[m]] = zs[m]; + done[ring[m]] = 1; + } + for (const auto k : ring) { + push_neighbours(static_cast(k % side), static_cast(k / side)); + } + } + return out; +} + +std::string origin_file_for(const std::string & navmap_path) +{ + const auto dot = navmap_path.find_last_of('.'); + const auto slash = navmap_path.find_last_of('/'); + const bool has_ext = dot != std::string::npos && (slash == std::string::npos || dot > slash); + return (has_ext ? navmap_path.substr(0, dot) : navmap_path) + ".yaml"; +} + +bool save_origin(const std::string & navmap_path, const MapOrigin & origin, double resolution) +{ + std::ofstream f(origin_file_for(navmap_path), std::ios::trunc); + if (!f) {return false;} + f.precision(10); + f << "# Position of the map's (0, 0, 0). Without this file a map is loaded as a plain NavMap\n" + << "# without geolocation.\n" + << "georeferenced: " << (origin.georeferenced ? "true" : "false") << "\n" + << "origin_lat: " << origin.lat << "\n" + << "origin_lon: " << origin.lon << "\n" + << "reference_altitude: " << origin.altitude << "\n" + << "min_resolution: " << resolution << "\n"; + return static_cast(f); +} + +MapOrigin load_origin(const std::string & navmap_path) +{ + MapOrigin o; + std::ifstream f(origin_file_for(navmap_path)); + if (!f) {return o;} + bool has_lat = false, has_lon = false; + std::string line; + while (std::getline(f, line)) { + const auto hash = line.find('#'); + if (hash != std::string::npos) {line.erase(hash);} + const auto colon = line.find(':'); + if (colon == std::string::npos) {continue;} + auto trim = [](std::string s) { + const auto b = s.find_first_not_of(" \t\r\n"); + const auto e = s.find_last_not_of(" \t\r\n"); + return b == std::string::npos ? std::string() : s.substr(b, e - b + 1); + }; + const std::string key = trim(line.substr(0, colon)), val = trim(line.substr(colon + 1)); + try { + if (key == "georeferenced") { + o.georeferenced = val == "true"; + } else if (key == "origin_lat") { + o.lat = std::stod(val); + has_lat = true; + } else if (key == "origin_lon") { + o.lon = std::stod(val); + has_lon = true; + } else if (key == "reference_altitude") { + o.altitude = std::stod(val); + } + } catch (const std::exception &) { + return MapOrigin{}; + } + } + // (a "nan" or "inf" reads as a number: not a place on Earth, so not georeferenced) + o.georeferenced = o.georeferenced && has_lat && has_lon && std::isfinite(o.lat) && + std::isfinite(o.lon) && std::isfinite(o.altitude); + return o; +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/navmap_filter_base.cpp b/maps_managers/easynav_bigmap_maps_manager/src/navmap_filter_base.cpp new file mode 100644 index 00000000..31b7e8a1 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/navmap_filter_base.cpp @@ -0,0 +1,55 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Out-of-line members of easynav::navmap::NavMapFilter, copied from easynav_navmap_maps_manager. +// +// Why they are here: BigMapMapsManager loads that package's filters (ObstaclesFilter, +// InflationFilter) through pluginlib. If this plugin were *linked* against +// libeasynav_navmap_maps_manager.so, the dynamic linker would load it first and pluginlib's +// ClassLoader would later find no factories for its classes ("no factory exists"). So we only +// use that package's headers and provide these four trivial members ourselves; the filter +// classes in the other library keep working on the same ABI. +#include +#include + +#include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" + +namespace easynav +{ +namespace navmap +{ + +NavMapFilter::NavMapFilter() {} + +void NavMapFilter::initialize( + const std::shared_ptr parent_node, + const std::string & plugin_name) +{ + parent_node_ = parent_node; + plugin_name_ = plugin_name; + on_initialize(); +} + +std::shared_ptr NavMapFilter::get_node() const +{ + return parent_node_.lock(); +} + +const std::string & NavMapFilter::get_plugin_name() const +{ + return plugin_name_; +} + +} // namespace navmap +} // namespace easynav diff --git a/maps_managers/easynav_bigmap_maps_manager/src/obstacle_memory.cpp b/maps_managers/easynav_bigmap_maps_manager/src/obstacle_memory.cpp new file mode 100644 index 00000000..dce3ff95 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/obstacle_memory.cpp @@ -0,0 +1,69 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/obstacle_memory.hpp" + +#include +#include + +namespace bigmap_maps_manager +{ + +std::int64_t ObstacleMemory::key(double x, double y, double z) const +{ + const auto ix = static_cast(std::floor(x / cell_)) & 0xfffff; + const auto iy = static_cast(std::floor(y / cell_)) & 0xfffff; + const auto iz = static_cast(std::lround(z)) & 0xfffff; // a band per metre + return (ix << 40) | (iy << 20) | iz; +} + +bool ObstacleMemory::add(const std::vector & detections, double now) +{ + bool changed = false; + for (const auto & d : detections) { + auto it = cells_.find(key(d.x, d.y, d.surface)); + if (it == cells_.end()) { + cells_[key(d.x, d.y, d.surface)] = {d.x, d.y, d.surface, 1, now}; + changed = true; + continue; + } + // A spot holds several detection cells, each with its own run, started at different moments: + // they report their periods at different times. One hit per spot per period (with 10 % + // allowance for the update rate), so max_hits hits need about max_hits x min_time seconds + if (now - it->second.last < 0.9 * min_time_ || it->second.hits >= max_hits_) {continue;} + ++it->second.hits; + it->second.last = now; + changed = true; + } + return changed; +} + +void ObstacleMemory::restore(double x, double y, double z, int hits) +{ + hits = std::clamp(hits, 1, max_hits_); + auto & cell = cells_[key(x, y, z)]; + if (cell.hits == 0) { + cell = {static_cast(x), static_cast(y), static_cast(z), 0, + -1e9}; + } + cell.hits = std::max(cell.hits, hits); +} + +std::size_t ObstacleMemory::fixed_count() const +{ + return static_cast(std::count_if( + cells_.begin(), cells_.end(), [this](const auto & kv) {return fixed(kv.second);})); +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/pipeline_check.cpp b/maps_managers/easynav_bigmap_maps_manager/src/pipeline_check.cpp new file mode 100644 index 00000000..7dda50e6 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/pipeline_check.cpp @@ -0,0 +1,112 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/pipeline_check.hpp" + +#include +#include + +namespace bigmap_maps_manager +{ + +ObstacleDetector parse_detector(const std::string & value) +{ + if (value == "bigmap_dynamic" || value == "dynamic") {return ObstacleDetector::BigmapDynamic;} + if (value == "navmap_filter" || value == "filter") {return ObstacleDetector::NavmapFilter;} + throw std::invalid_argument( + "obstacles.detector: bigmap_dynamic or navmap_filter, not '" + value + "'"); +} + +const char * to_string(ObstacleDetector detector) +{ + return detector == ObstacleDetector::BigmapDynamic ? "bigmap_dynamic" : "navmap_filter"; +} + +PipelineReport check_pipeline(const PipelineSettings & s) +{ + PipelineReport r; + const auto lists = [&s](const char * layer) { + return std::any_of( + s.filters.begin(), s.filters.end(), + [layer](const LoadedFilter & f) {return f.layer == layer;}); + }; + const bool obstacle_filter = lists("obstacles"); + const bool inflation = lists("inflated_obstacles"); + const bool bigmap_detects = s.detector == ObstacleDetector::BigmapDynamic; + + if (!bigmap_detects && !obstacle_filter) { + r.errors.push_back( + "obstacles.detector: navmap_filter, but no obstacle filter is in 'filters' (an entry whose " + "plugin is ...ObstaclesFilter, named other than 'obstacles'): the sensors would never " + "detect anything. List it, or use obstacles.detector: bigmap_dynamic"); + } + { + // the inflation spreads what is in `obstacles` when it runs: listed before the obstacle + // filter it always works on the previous cycle's obstacles + std::size_t first_inflation = s.filters.size(), last_obstacle = 0; + for (std::size_t k = 0; k < s.filters.size(); ++k) { + if (s.filters[k].layer == "inflated_obstacles") { + first_inflation = std::min(first_inflation, k); + } + if (s.filters[k].layer == "obstacles") {last_obstacle = k;} + } + if (obstacle_filter && inflation && first_inflation < last_obstacle) { + r.warnings.push_back( + "'filters' lists the inflation before the obstacle filter: it inflates the obstacles of " + "the previous cycle. List the obstacle filter first"); + } + } + if (bigmap_detects && obstacle_filter) { + r.warnings.push_back( + "obstacles.detector: bigmap_dynamic, and an obstacle filter is listed in 'filters' too: " + "both write the 'obstacles' layer and the filter's marks are never removed from the map. " + "Set obstacles.detector: navmap_filter or take the filter out of 'filters'"); + } + + r.sensors_processed = bigmap_detects && (s.dynamic_map_enabled || s.memory_enabled); + r.memory_effective = bigmap_detects && s.memory_enabled; + if (!bigmap_detects && s.memory_enabled) { + r.warnings.push_back( + "obstacles.memory.enabled needs obstacles.detector: bigmap_dynamic (the filter's obstacles " + "carry no detection time): the memory is off"); + } + if (bigmap_detects && !r.sensors_processed) { + r.notes.push_back( + "no sensor obstacles: dynamic_map.enabled and obstacles.memory.enabled are both false; " + "only terrain steeper than terrain.max_slope_deg blocks"); + } + if (r.sensors_processed && !s.removal_enabled) { + r.warnings.push_back( + "obstacles.removal.enabled is false: what the sensors detect reaches the dynamic map but " + "NOT the 'obstacles' layer, so the planner does not see it"); + } + r.planner_layer = inflation ? "inflated_obstacles" : "obstacles"; + if (!inflation) { + r.notes.push_back( + "no inflation filter in 'filters': A* reads 'obstacles' and keeps no distance to obstacles"); + } + + std::string loaded; + for (const auto & f : s.filters) { + loaded += (loaded.empty() ? "" : ", ") + f.instance; + } + r.notes.push_back( + std::string("obstacles: detector ") + to_string(s.detector) + ", sensors " + + (r.sensors_processed || !bigmap_detects ? "analysed" : "not analysed") + ", memory " + + (r.memory_effective ? "on" : "off") + ", removal " + (s.removal_enabled ? "on" : "off") + + "; filters [" + loaded + "]; A* reads '" + r.planner_layer + "'"); + return r; +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/rtin_error_map.cpp b/maps_managers/easynav_bigmap_maps_manager/src/rtin_error_map.cpp new file mode 100644 index 00000000..558cc8b0 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/rtin_error_map.cpp @@ -0,0 +1,122 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/rtin_error_map.hpp" + +#include +#include +#include +#include + +namespace bigmap_maps_manager +{ + +std::uint16_t RtinErrorMap::quantize(float e) +{ + // rounded up: an error is never stored smaller than it is (a NaN height: the largest). Called + // once per RTIN triangle (half a billion on a 16k grid): a multiplication and an integer + // round-up, not a division and std::ceil (a libm call without SSE4.1) + const float q = e * (1.0f / kErrorStep); + if (!(q < 65535.0f)) {return 65535;} + if (!(q > 0.0f)) {return 0;} + const auto i = static_cast(q); + return static_cast(static_cast(i) < q ? i + 1 : i); +} + +RtinErrorMap::RtinErrorMap( + const DemGrid & dem, const std::vector & pinned, + const std::vector & hole_nodes, double hole_error) +: dem_(&dem) +{ + if (dem.side < 3) { + throw std::invalid_argument("DEM too small for RTIN"); + } + real_side_ = dem.side; + tile_size_ = 1; + while (tile_size_ + 1 < real_side_) { + tile_size_ <<= 1; + } + const int ts = tile_size_; + const int gs = grid_size(); + const std::size_t n = static_cast(gs) * gs; + + errors_.assign(n, 0); + if (hole_error > 0.0 && hole_nodes.size() == dem.z.size()) { + // the border of a hole counts as an error of hole_error: propagated up with the rest + const std::uint16_t he = quantize(static_cast(hole_error)); + for (int y = 0; y < real_side_; ++y) { + for (int x = 0; x < real_side_; ++x) { + if (hole_nodes[static_cast(y) * real_side_ + x]) {errors_[idx(x, y)] = he;} + } + } + } + const bool any_pinned = std::find(pinned.begin(), pinned.end(), 1) != pinned.end(); + if (any_pinned) { + pinned_.assign(n, 0); + for (int y = 0; y < real_side_; ++y) { + for (int x = 0; x < real_side_; ++x) { + pinned_[idx(x, y)] = pinned[static_cast(y) * real_side_ + x] ? 1 : 0; + } + } + } + + const long num_triangles = static_cast(ts) * ts * 2 - 2; // NOLINT + const long num_parents = num_triangles - static_cast(ts) * ts; // NOLINT + + // Walk the triangle tree from the smallest triangles up so children are done before parents. + for (long i = num_triangles - 1; i >= 0; --i) { // NOLINT + long id = i + 2; // NOLINT + int ax = 0, ay = 0, bx = 0, by = 0, cx = 0, cy = 0; + if (id & 1) { + bx = by = cx = ts; + } else { + ax = ay = cy = ts; + } + while ((id >>= 1) > 1) { + const int mx = (ax + bx) >> 1; + const int my = (ay + by) >> 1; + if (id & 1) { + bx = ax; by = ay; ax = cx; ay = cy; + } else { + ax = bx; ay = by; bx = cx; by = cy; + } + cx = mx; cy = my; + } + + const int mx = (ax + bx) >> 1; + const int my = (ay + by) >> 1; + cx = mx + my - ay; + cy = my + ax - mx; + + const float interpolated = (height(ax, ay) + height(bx, by)) * 0.5f; + const std::size_t mid = idx(mx, my); + errors_[mid] = std::max(errors_[mid], quantize(std::abs(interpolated - height(mx, my)))); + + if (any_pinned) { + if (i < num_parents) { + pinned_[mid] |= pinned_[idx((ax + cx) >> 1, (ay + cy) >> 1)] | + pinned_[idx((bx + cx) >> 1, (by + cy) >> 1)]; + } else { + pinned_[mid] |= pinned_[idx(ax, ay)] | pinned_[idx(bx, by)] | pinned_[idx(cx, cy)]; + } + } + if (i < num_parents) { + const std::size_t left = idx((ax + cx) >> 1, (ay + cy) >> 1); + const std::size_t right = idx((bx + cx) >> 1, (by + cy) >> 1); + errors_[mid] = std::max({errors_[mid], errors_[left], errors_[right]}); + } + } +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/src/surface_lod.cpp b/maps_managers/easynav_bigmap_maps_manager/src/surface_lod.cpp new file mode 100644 index 00000000..a3818a65 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/surface_lod.cpp @@ -0,0 +1,1331 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "easynav_bigmap_maps_manager/surface_lod.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace bigmap_maps_manager +{ + +namespace +{ +struct WeldKey +{ + std::int32_t a, b, c; + bool operator==(const WeldKey & o) const {return a == o.a && b == o.b && c == o.c;} +}; +struct WeldHash +{ + std::size_t operator()(const WeldKey & k) const + { + std::size_t h = static_cast(k.a) * 73856093u; + h ^= static_cast(k.b) * 19349663u; + h ^= static_cast(k.c) * 83492791u; + return h; + } +}; +struct TriHash +{ + std::size_t operator()(const std::array & t) const + { + std::size_t h = t[0] * 73856093u; + h ^= t[1] * 19349663u; + h ^= t[2] * 83492791u; + return h; + } +}; + +// The cell index of a coordinate. An absurd one (a robot pose of 1e12 m, a vertex of 3e38 m) is +// clamped first: its cast to int64 is undefined and the index arithmetic around it overflows. +std::int64_t cell_index(double v, double cell) +{ + return static_cast(std::floor(std::clamp(v / cell, -1e15, 1e15))); +} + +// (shifted as unsigned: a negative cell index shifted left is undefined before C++20) +std::int64_t cell_key_of(std::int64_t ix, std::int64_t iy) +{ + return static_cast(static_cast(ix) << 32) ^ (iy & 0xffffffffLL); +} + +std::int64_t cell_key(double x, double y, double cell) +{ + return cell_key_of(cell_index(x, cell), cell_index(y, cell)); +} + +std::int64_t key_x(std::int64_t key) {return key >> 32;} +std::int64_t key_y(std::int64_t key) +{ + return static_cast(static_cast(key & 0xffffffffLL)); +} + +// Slopes are kept as hundredths of a degree, rounded up: never gentler than the source's +std::uint16_t slope_code(double degrees) +{ + return static_cast(std::clamp(std::ceil(degrees * 100.0 - 1e-6), 0.0, 65535.0)); +} + +// An upper bound of the effective distances at which the policy splits a node of size `s` and +// error `err` (negative: it never does). Beyond it the node's decision is the one of a robot +// infinitely far, the far cut's: the `dynamic radius` of the node. +double split_radius_of(double s, double err, const LodParams & p) +{ + if (!(s > p.min_size)) {return -1.0;} + const double hr = p.high_res_radius, lr = p.low_res_distance; + // the size cap reaches s here (it is min_size up to high_res_radius, max_size from + // low_res_distance on, geometric in between) + double r = std::max(hr, lr); + if (lr > hr && p.max_size > p.min_size && s <= p.max_size) { + r = hr + (lr - hr) * std::log(s / p.min_size) / std::log(p.max_size / p.min_size); + } + // the tolerance reaches err here + if (err > p.base_error && p.error_growth > 0.0) { + r = std::max(r, hr + (err - p.base_error) / p.error_growth); + } + return r * (1.0 + 1e-6) + 1e-3; +} + +bool finite_params(const LodParams & p) +{ + for (const double v : {p.high_res_radius, p.low_res_distance, p.min_size, p.max_size, + p.base_error, p.error_growth, p.known_margin, p.known_falloff}) + { + if (!std::isfinite(v)) {return false;} + } + return true; +} +} // namespace + +struct SurfaceLodMesher::Query +{ + const LodParams & p; + LodParams po; // the policy for the observed ground + double rx, ry, rz; +}; + +// --------------------------------------------------------------------------------------------- +// CellIndex + +void SurfaceLodMesher::CellIndex::build( + double cell_size, const std::vector & x, const std::vector & y) +{ + cell = cell_size; + const std::size_t n = x.size(); + std::vector> kv(n); + for (std::size_t i = 0; i < n; ++i) { + kv[i] = {cell_key(x[i], y[i], cell), static_cast(i)}; + } + std::sort(kv.begin(), kv.end()); + keys.clear(); + start.clear(); + items.resize(n); + for (std::size_t i = 0; i < n; ++i) { + if (i == 0 || kv[i].first != kv[i - 1].first) { + keys.push_back(kv[i].first); + start.push_back(static_cast(i)); + } + items[i] = kv[i].second; + } + start.push_back(static_cast(n)); + keys.shrink_to_fit(); + start.shrink_to_fit(); +} + +template +void SurfaceLodMesher::CellIndex::visit_cell(std::int64_t key, F && f) const +{ + const auto it = std::lower_bound(keys.begin(), keys.end(), key); + if (it == keys.end() || *it != key) {return;} + const auto i = static_cast(it - keys.begin()); + for (std::uint32_t m = start[i]; m < start[i + 1]; ++m) { + f(items[m]); + } +} + +template +void SurfaceLodMesher::CellIndex::visit_disc(double x, double y, double radius, F && f) const +{ + if (keys.empty() || !(radius >= 0.0)) {return;} + const auto span = static_cast(std::ceil(std::min(radius, 1e12) / cell)); + const auto cx = cell_index(x, cell), cy = cell_index(y, cell); + const double cells = static_cast(2 * span + 1) * static_cast(2 * span + 1); + if (cells > 4.0 * static_cast(keys.size())) { + // a disc wider than the map: every occupied cell in the square + for (std::size_t i = 0; i < keys.size(); ++i) { + if (std::abs(key_x(keys[i]) - cx) > span || std::abs(key_y(keys[i]) - cy) > span) {continue;} + for (std::uint32_t m = start[i]; m < start[i + 1]; ++m) { + f(items[m]); + } + } + return; + } + for (std::int64_t j = cy - span; j <= cy + span; ++j) { + for (std::int64_t i = cx - span; i <= cx + span; ++i) { + visit_cell(cell_key_of(i, j), f); + } + } +} + +std::size_t SurfaceLodMesher::CellIndex::memory_bytes() const +{ + return keys.capacity() * sizeof(std::int64_t) + + (start.capacity() + items.capacity()) * sizeof(std::uint32_t); +} + +// --------------------------------------------------------------------------------------------- +// Construction + +SurfaceLodMesher::SurfaceLodMesher(const SourceMesh & map, const Config & config) +: cfg_(config) +{ + init(map); +} + +SurfaceLodMesher::SurfaceLodMesher(const ::navmap::NavMap & map, const Config & config) +: cfg_(config) +{ + if (map.navcels.empty() || map.positions.x.empty()) { + throw std::invalid_argument("the NavMap has no NavCels"); + } + init(SourceMesh::from_navmap(map)); +} + +void SurfaceLodMesher::init(const SourceMesh & map) +{ + if (map.triangle_count() == 0 || map.x.empty()) { + throw std::invalid_argument("the NavMap has no NavCels"); + } + if (!(cfg_.min_size > 0.0) || !(cfg_.max_size >= cfg_.min_size) || + !(cfg_.cluster_height > 0.0) || !(cfg_.hole_error >= 0.0) || !std::isfinite(cfg_.max_size) || + !std::isfinite(cfg_.hole_error)) + { + throw std::invalid_argument("invalid surface LOD configuration"); + } + // (a NaN here would be welded to one vertex, cast to a cell index (undefined) and come out in + // every mesh; an index out of range read anywhere) + map.validate(); + const auto & px = map.x; + const auto & py = map.y; + const auto & pz = map.z; + coloured_ = map.coloured(); + + // Weld the vertices that share a position (to a millimetre): the clusters need a connected mesh + Level & l0 = levels_.emplace_back(); + std::vector remap(px.size(), 0); + { + std::unordered_map welded; + welded.reserve(px.size()); + for (std::size_t v = 0; v < px.size(); ++v) { + const WeldKey k{static_cast(std::lround(px[v] * 1000.0)), + static_cast(std::lround(py[v] * 1000.0)), + static_cast(std::lround(pz[v] * 1000.0))}; + const auto it = welded.emplace(k, static_cast(l0.x.size())); + remap[v] = it.first->second; + if (!it.second) {continue;} + l0.x.push_back(px[v]); + l0.y.push_back(py[v]); + l0.z.push_back(pz[v]); + if (coloured_) { + l0.r.push_back(map.r[v]); + l0.g.push_back(map.g[v]); + l0.b.push_back(map.b[v]); + } + } + } + const std::size_t V = l0.x.size(); + known_.assign(V, 0); + + had_traversed_ = map.has_traversed_layer; + const std::size_t nt_src = map.triangle_count(); + tri_.reserve(nt_src * 3); + tri_slope_.reserve(nt_src); + for (std::size_t t = 0; t < nt_src; ++t) { + const std::uint32_t a = remap[map.v0[t]], b = remap[map.v1[t]], d = remap[map.v2[t]]; + if (a == b || b == d || a == d) {continue;} + tri_.insert(tri_.end(), {a, b, d}); + { + const double ux = l0.x[b] - l0.x[a], uy = l0.y[b] - l0.y[a], uz = l0.z[b] - l0.z[a]; + const double vx = l0.x[d] - l0.x[a], vy = l0.y[d] - l0.y[a], vz = l0.z[d] - l0.z[a]; + const double nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx; + const double len = std::sqrt(nx * nx + ny * ny + nz * nz); + tri_slope_.push_back(slope_code(len < 1e-12 ? 90.0 : + std::acos(std::min(1.0, std::abs(nz) / len)) * 180.0 / M_PI)); + } + if (!map.traversed.empty() && map.traversed[t] > 0) { + for (const auto v : {a, b, d}) { + known_[v] = 1; + } + } + } + remap = {}; + if (tri_.empty()) {throw std::invalid_argument("the NavMap has no usable NavCels");} + + if (cfg_.drop_unreachable_ground) { + // union-find over the triangles that share an edge + const std::size_t nt = tri_.size() / 3; + std::vector root(nt); + for (std::uint32_t t = 0; t < nt; ++t) { + root[t] = t; + } + const auto find = [&root](std::uint32_t t) { + while (root[t] != t) {root[t] = root[root[t]]; t = root[t];} + return t; + }; + { + std::unordered_map first; + first.reserve(tri_.size()); + for (std::uint32_t t = 0; t < nt; ++t) { + for (int k = 0; k < 3; ++k) { + auto a = tri_[3 * t + k], b = tri_[3 * t + (k + 1) % 3]; + if (a > b) {std::swap(a, b);} + const auto key = (static_cast(a) << 32) | b; + const auto it = first.emplace(key, t); + if (!it.second) {root[find(t)] = find(it.first->second);} + } + } + } + std::unordered_map size; + for (std::uint32_t t = 0; t < nt; ++t) { + ++size[find(t)]; + } + std::uint32_t best = find(0); + for (const auto & kv : size) { + if (kv.second > size[best]) {best = kv.first;} + } + std::size_t kept = 0; + for (std::uint32_t t = 0; t < nt; ++t) { + if (find(t) != best) {continue;} + for (int k = 0; k < 3; ++k) { + tri_[3 * kept + k] = tri_[3 * t + k]; + } + tri_slope_[kept] = tri_slope_[t]; + ++kept; + } + n_dropped_ = nt - kept; + tri_.resize(3 * kept); + tri_slope_.resize(kept); + // what was driven over on the dropped pieces does not count + std::vector used(V, 0); + for (const auto v : tri_) { + used[v] = 1; + } + for (std::size_t v = 0; v < V; ++v) { + if (!used[v]) {known_[v] = 0;} + } + } + tri_.shrink_to_fit(); + tri_slope_.shrink_to_fit(); + for (std::uint32_t v = 0; v < V; ++v) { + if (known_[v]) {known_list_.push_back(v);} + } + n_known_ = known_list_.size(); + observed_.assign(V, 0); + + // The source triangles of each vertex (CSR) + vtx_tri_start_.assign(V + 1, 0); + for (const auto v : tri_) { + ++vtx_tri_start_[v + 1]; + } + for (std::size_t v = 0; v < V; ++v) { + vtx_tri_start_[v + 1] += vtx_tri_start_[v]; + } + vtx_tri_.resize(tri_.size()); + { + std::vector fill(vtx_tri_start_.begin(), vtx_tri_start_.end() - 1); + for (std::size_t i = 0; i < tri_.size(); ++i) { + vtx_tri_[fill[tri_[i]]++] = static_cast(i / 3); + } + } + + // The border of the mesh: the ends of the edges that belong to one triangle only + l0.flags.assign(V, 0); + { + std::unordered_map uses; + uses.reserve(tri_.size()); + const auto edge_key = [](std::uint32_t a, std::uint32_t b) { + if (a > b) {std::swap(a, b);} + return (static_cast(a) << 32) | b; + }; + for (std::size_t t = 0; t < tri_.size(); t += 3) { + for (int k = 0; k < 3; ++k) { + ++uses[edge_key(tri_[t + k], tri_[t + (k + 1) % 3])]; + } + } + for (std::size_t t = 0; t < tri_.size(); t += 3) { + for (int k = 0; k < 3; ++k) { + const auto a = tri_[t + k], b = tri_[t + (k + 1) % 3]; + if (uses[edge_key(a, b)] == 1) {l0.flags[a] |= kBoundary; l0.flags[b] |= kBoundary;} + } + } + } + grid_.build(kGridCell, l0.x, l0.y); + if (cfg_.keep_narrow_width > 0.0) {pin_narrow_passages();} + // Typical edge of the source: clustering finer than two of them only blurs the mesh (holes, steps + // across a narrow ramp). The first cluster level is never smaller, and wherever the policy asks + // for less than that the mesh is the source itself. + { + std::vector edge; + edge.reserve(tri_.size() / 3); + for (std::size_t t = 0; t < tri_.size(); t += 3) { + const auto a = tri_[t], b = tri_[t + 1], c = tri_[t + 2]; + const auto len = [&l0](std::uint32_t p, std::uint32_t q) { + return std::hypot(l0.x[p] - l0.x[q], l0.y[p] - l0.y[q], l0.z[p] - l0.z[q]); + }; + edge.push_back(static_cast((len(a, b) + len(b, c) + len(c, a)) / 3.0)); + } + std::nth_element(edge.begin(), edge.begin() + edge.size() / 2, edge.end()); + source_edge_ = edge[edge.size() / 2]; + } + build_levels(); + + offset_.assign(levels_.size() + 1, 0); + for (std::size_t k = 0; k < levels_.size(); ++k) { + offset_[k + 1] = offset_[k] + levels_[k].x.size(); + } + if (offset_.back() >= kNone) {throw std::invalid_argument("the NavMap is too large");} + node_grid_.resize(levels_.size()); + for (std::size_t k = 1; k < levels_.size(); ++k) { + node_grid_[k].build(std::max(4.0 * levels_[k].size, kGridCell), levels_[k].x, levels_[k].y); + } +} + +void SurfaceLodMesher::pin_narrow_passages() +{ + // Distance of every vertex to the nearest border vertex (same floor: within a metre in z); the + // half-width of the passage around a vertex is the largest such distance in its surroundings. A + // vertex whose surroundings are all closer to a border than half the width is in a narrow one. + Level & l0 = levels_[0]; + const std::size_t n = l0.x.size(); + const double half = 0.5 * cfg_.keep_narrow_width; + const double reach = half + 0.5; // the borders that matter and the surroundings to look at + std::vector to_border(n, static_cast(reach + 1.0)); + for (std::uint32_t v = 0; v < n; ++v) { + if (l0.flags[v] & kBoundary) {to_border[v] = 0.0f; continue;} + grid_.visit_disc(l0.x[v], l0.y[v], reach, [&](std::uint32_t u) { + if (!(l0.flags[u] & kBoundary) || std::abs(l0.z[u] - l0.z[v]) > 1.0) {return;} + const double d = std::hypot(l0.x[u] - l0.x[v], l0.y[u] - l0.y[v]); + if (d < to_border[v]) {to_border[v] = static_cast(d);} + }); + } + for (std::uint32_t v = 0; v < n; ++v) { + float widest = 0.0f; + const double look = half + 0.25; // far enough to reach the middle of a wide passage + grid_.visit_disc(l0.x[v], l0.y[v], look, [&](std::uint32_t u) { + if (std::abs(l0.z[u] - l0.z[v]) > 1.0 || + std::hypot(l0.x[u] - l0.x[v], l0.y[u] - l0.y[v]) > look) {return;} + widest = std::max(widest, to_border[u]); + }); + if (widest < half) { + l0.flags[v] |= kPinned; + ++n_pinned_; + } + } +} + +void SurfaceLodMesher::build_levels() +{ + // The edges of the source mesh (each once): two nodes are merged only if the mesh joins them + // inside the cell. Without it the two sides of a gap, of a wall's footprint or of a ledge + // falling in one cell become one node, and the coarse mesh bridges what cannot be driven. + std::vector> edges; + { + std::unordered_set seen; + seen.reserve(tri_.size()); + for (std::size_t t = 0; t < tri_.size(); t += 3) { + for (int k = 0; k < 3; ++k) { + auto a = tri_[t + k], b = tri_[t + (k + 1) % 3]; + if (a > b) {std::swap(a, b);} + if (seen.insert((static_cast(a) << 32) | b).second) { + edges.emplace_back(a, b); + } + } + } + } + // What only the construction needs, per node of the level being grouped + struct Build + { + std::vector zmin, zmax, r, g, b; + std::vector count; + }; + Build prev_b; + { + const Level & l0 = levels_[0]; + prev_b.zmin = l0.z; + prev_b.zmax = l0.z; + prev_b.count.assign(l0.x.size(), 1); + if (coloured_) { + prev_b.r.assign(l0.r.begin(), l0.r.end()); + prev_b.g.assign(l0.g.begin(), l0.g.end()); + prev_b.b.assign(l0.b.begin(), l0.b.end()); + } + } + // owner[v]: the node of the previous level that holds source vertex v + std::vector owner(levels_[0].x.size()); + for (std::uint32_t v = 0; v < owner.size(); ++v) { + owner[v] = v; + } + std::vector uf; // union-find over the nodes of the previous level + const auto find = [&uf](std::uint32_t a) { + while (uf[a] != a) { + uf[a] = uf[uf[a]]; + a = uf[a]; + } + return a; + }; + + double size = std::max(cfg_.min_size, 2.0 * source_edge_); + while (true) { + Level & prev = levels_.back(); + const std::size_t n = prev.x.size(); + Level next; + Build next_b; + const auto key_of = [&](std::uint32_t i) { + // border nodes and inner ones are never mixed + // (shifted as unsigned: the key is negative for cells west or south of the origin) + return static_cast( + (static_cast(cell_key(prev.x[i], prev.y[i], size)) << 1) | + static_cast(prev.flags[i] & kBoundary)); + }; + uf.resize(n); + for (std::uint32_t i = 0; i < n; ++i) { + uf[i] = i; + } + for (const auto & [a, b] : edges) { + const auto na = owner[a], nb = owner[b]; + if (na != nb && key_of(na) == key_of(nb)) {uf[find(na)] = find(nb);} + } + // The groups of this level. `locked` nodes of the previous level stay alone (see the fold-over + // check below). + const auto group = [&](const std::vector & locked) { + next = Level(); + next_b = Build(); + next.size = size; + // the nodes of the previous level by cell + std::unordered_map> cells; + for (std::uint32_t i = 0; i < n; ++i) { + cells[key_of(i)].push_back(i); + } + prev.parent.assign(n, 0); + for (auto & kv : cells) { + auto & ids = kv.second; + // by connected piece, and inside each piece by height + std::vector piece(ids.size()); + for (std::size_t m = 0; m < ids.size(); ++m) { + piece[m] = find(ids[m]); + } + std::vector order(ids.size()); + for (std::size_t m = 0; m < order.size(); ++m) { + order[m] = m; + } + std::sort(order.begin(), order.end(), [&](std::size_t a, std::size_t b) { + return piece[a] != piece[b] ? piece[a] < piece[b] : prev.z[ids[a]] < prev.z[ids[b]]; + }); + { + std::vector sorted_ids(ids.size()), sorted_piece(ids.size()); + for (std::size_t m = 0; m < order.size(); ++m) { + sorted_ids[m] = ids[order[m]]; + sorted_piece[m] = piece[order[m]]; + } + ids.swap(sorted_ids); + piece.swap(sorted_piece); + } + std::size_t i = 0; + while (i < ids.size()) { + // a group: consecutive heights of one piece that together span at most cluster_height + float lo = prev_b.zmin[ids[i]], hi = prev_b.zmax[ids[i]]; + std::size_t j = i + 1; + while (j < ids.size() && piece[j] == piece[i] && !locked[ids[i]] && !locked[ids[j]]) { + const float nlo = std::min(lo, prev_b.zmin[ids[j]]); + const float nhi = std::max(hi, prev_b.zmax[ids[j]]); + if (nhi - nlo > cfg_.cluster_height) {break;} + lo = nlo; + hi = nhi; + ++j; + } + const auto id = static_cast(next.x.size()); + double sx = 0, sy = 0, sz = 0, sr = 0, sg = 0, sb = 0, w = 0; + std::uint8_t pin = 0; + for (std::size_t m = i; m < j; ++m) { + const auto c = ids[m]; + const double cw = prev_b.count[c]; + sx += cw * prev.x[c]; + sy += cw * prev.y[c]; + sz += cw * prev.z[c]; + if (coloured_) { + sr += cw * prev_b.r[c]; + sg += cw * prev_b.g[c]; + sb += cw * prev_b.b[c]; + } + w += cw; + pin = pin || (prev.flags[c] & kPinned); + prev.parent[c] = id; + } + next.x.push_back(static_cast(sx / w)); + next.y.push_back(static_cast(sy / w)); + next.z.push_back(static_cast(sz / w)); + next_b.zmin.push_back(lo); + next_b.zmax.push_back(hi); + next_b.count.push_back(static_cast(w)); + next.flags.push_back(static_cast( + (prev.flags[ids[i]] & kBoundary) | (pin ? kPinned : 0))); + if (coloured_) { + next_b.r.push_back(static_cast(sr / w)); + next_b.g.push_back(static_cast(sg / w)); + next_b.b.push_back(static_cast(sb / w)); + } + i = j; + } + } + }; + // A merge must not fold the surface over itself: a source triangle whose three corners end up + // in three different nodes keeps its orientation (seen from above). Where one flips, its + // corners' groups are undone and the level is built again (a few rounds at most). + std::vector locked(n, 0); + for (int round = 0; round < 4; ++round) { + group(locked); + bool flipped = false; + for (std::size_t t = 0; t < tri_.size(); t += 3) { + const std::uint32_t a = tri_[t], b = tri_[t + 1], c = tri_[t + 2]; + const auto na = prev.parent[owner[a]], nb = prev.parent[owner[b]], + nc = prev.parent[owner[c]]; + if (na == nb || nb == nc || na == nc) {continue;} + const auto & l0 = levels_[0]; + const auto cross = [](double ax, double ay, double bx, double by, double cx, double cy) { + return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax); + }; + const double src = cross(l0.x[a], l0.y[a], l0.x[b], l0.y[b], l0.x[c], l0.y[c]); + const double now = cross(next.x[na], next.y[na], next.x[nb], next.y[nb], next.x[nc], + next.y[nc]); + if (std::abs(src) > 1e-9 && src * now < 0.0) { + for (const auto v : {a, b, c}) { + locked[owner[v]] = 1; + } + flipped = true; + } + } + if (!flipped) {break;} + } + for (auto & o : owner) { + o = prev.parent[o]; + } + // the node's error (the height it spans around its mean), its colour, its children + const std::size_t m = next.x.size(); + next.err.resize(m); + for (std::size_t i = 0; i < m; ++i) { + float e = std::max(next_b.zmax[i] - next.z[i], next.z[i] - next_b.zmin[i]); + if (cfg_.hole_error > 0.0 && (next.flags[i] & kBoundary)) { + e = std::max(e, static_cast(cfg_.hole_error)); + } + next.err[i] = e; + next.max_err = std::max(next.max_err, e); + } + if (coloured_) { + const auto to_u8 = [m](const std::vector & in, std::vector & out) { + out.resize(m); + for (std::size_t i = 0; i < m; ++i) { + out[i] = static_cast(std::clamp(std::lround(in[i]), 0L, 255L)); + } + }; + to_u8(next_b.r, next.r); + to_u8(next_b.g, next.g); + to_u8(next_b.b, next.b); + } + next.child_start.assign(m + 1, 0); + for (std::size_t c = 0; c < n; ++c) { + ++next.child_start[prev.parent[c] + 1]; + } + for (std::size_t i = 0; i < m; ++i) { + next.child_start[i + 1] += next.child_start[i]; + } + next.child.resize(n); + { + std::vector fill(next.child_start.begin(), next.child_start.end() - 1); + for (std::uint32_t c = 0; c < n; ++c) { + next.child[fill[prev.parent[c]]++] = c; + } + } + prev_b = std::move(next_b); + levels_.push_back(std::move(next)); + if (size >= cfg_.max_size || levels_.size() > 12) {break;} + size *= 2.0; + } +} + +// --------------------------------------------------------------------------------------------- +// Driven-over and observed ground + +int SurfaceLodMesher::mark(double x, double y, double z, double radius) +{ + // (a NaN cast to a cell index is undefined; nothing is there anyway) + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) || !(radius >= 0.0)) {return 0;} + const auto & l0 = levels_[0]; + int added = 0; + grid_.visit_disc(x, y, std::min(radius, 1e6), [&](std::uint32_t v) { + if (known_[v] || std::hypot(l0.x[v] - x, l0.y[v] - y) > radius || + std::abs(l0.z[v] - z) > cfg_.vertical_tolerance) + { + return; + } + known_[v] = 1; + known_list_.push_back(v); + ++added; + }); + if (added > 0) { + n_known_ = known_list_.size(); + kd_dirty_ = true; + } + return added; +} + +std::size_t SurfaceLodMesher::forget_outside(double x, double y, double z, double radius) +{ + if (known_list_.empty() || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) { + return 0; + } + const auto & l0 = levels_[0]; + std::size_t kept = 0; + for (const auto v : known_list_) { + const double dx = l0.x[v] - x, dy = l0.y[v] - y, dz = l0.z[v] - z; + if (std::sqrt(dx * dx + dy * dy + dz * dz) > radius) { + known_[v] = 0; + } else { + known_list_[kept++] = v; + } + } + const std::size_t forgotten = known_list_.size() - kept; + if (forgotten > 0) { + known_list_.resize(kept); + n_known_ = kept; + kd_dirty_ = true; + } + return forgotten; +} + +int SurfaceLodMesher::mark_observed(double x, double y, double z, double radius) +{ + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) || !(radius >= 0.0)) {return 0;} + const auto & l0 = levels_[0]; + int added = 0; + grid_.visit_disc(x, y, std::min(radius, 1e6), [&](std::uint32_t v) { + if (observed_[v] || !(l0.flags[v] & kBoundary) || + std::hypot(l0.x[v] - x, l0.y[v] - y) > radius || + std::abs(l0.z[v] - z) > cfg_.vertical_tolerance) + { + return; + } + observed_[v] = 1; + observed_list_.push_back(v); + ++added; + }); + if (added > 0) {kd_dirty_ = true;} + return added; +} + +std::size_t SurfaceLodMesher::forget_observed_outside( + double x, double y, double z, double radius) +{ + if (observed_list_.empty() || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) { + return 0; + } + const auto & l0 = levels_[0]; + std::size_t kept = 0; + for (const auto v : observed_list_) { + const double dx = l0.x[v] - x, dy = l0.y[v] - y, dz = l0.z[v] - z; + if (std::sqrt(dx * dx + dy * dy + dz * dz) > radius) { + observed_[v] = 0; + } else { + observed_list_[kept++] = v; + } + } + const std::size_t forgotten = observed_list_.size() - kept; + if (forgotten > 0) { + observed_list_.resize(kept); + kd_dirty_ = true; + } + return forgotten; +} + +// The driven-over and the observed vertices as balanced 2-D trees over (x, y): the nearest one +// (on the same floor: within vertical_tolerance in z) within `reach` of a node is found in +// O(log n). Scanning every 2 m cell within reach instead (a reach of 50 m: 2600 cells per node) +// took up to a second per re-mesh on a big building. +void SurfaceLodMesher::build_kd() const +{ + if (!kd_dirty_) {return;} + kd_dirty_ = false; + const auto & l0 = levels_[0]; + for (auto [list, kd] : {std::pair{&known_list_, &kd_known_}, + std::pair{&observed_list_, &kd_observed_}}) + { + kd->clear(); + kd->reserve(list->size()); + for (const auto v : *list) { + kd->push_back({l0.x[v], l0.y[v], l0.z[v], true}); + } + kd_build(*kd, 0, kd->size()); + } +} + +void SurfaceLodMesher::kd_build(std::vector & kd, std::size_t lo, std::size_t hi) +{ + // each node is the median of its range along the wider axis: [lo, mid) left, (mid, hi) right + if (hi - lo <= 1) {return;} + float min_x = kd[lo].x, max_x = min_x, min_y = kd[lo].y, max_y = min_y; + for (std::size_t i = lo + 1; i < hi; ++i) { + min_x = std::min(min_x, kd[i].x); + max_x = std::max(max_x, kd[i].x); + min_y = std::min(min_y, kd[i].y); + max_y = std::max(max_y, kd[i].y); + } + const bool by_x = max_x - min_x >= max_y - min_y; + const std::size_t mid = lo + (hi - lo) / 2; + std::nth_element( + kd.begin() + lo, kd.begin() + mid, kd.begin() + hi, + [by_x](const KdNode & a, const KdNode & b) {return by_x ? a.x < b.x : a.y < b.y;}); + kd[mid].by_x = by_x; + kd_build(kd, lo, mid); + kd_build(kd, mid + 1, hi); +} + +void SurfaceLodMesher::kd_nearest( + const std::vector & kd, std::size_t lo, std::size_t hi, double x, double y, double z, + double & best) const +{ + if (hi <= lo) {return;} + const std::size_t mid = lo + (hi - lo) / 2; + const KdNode & n = kd[mid]; + const double dx = x - n.x, dy = y - n.y, dz = z - n.z; + if (std::abs(dz) <= cfg_.vertical_tolerance) {best = std::min(best, dx * dx + dy * dy + dz * dz);} + const double delta = n.by_x ? dx : dy; // signed distance to the splitting line + if (delta < 0.0) { + kd_nearest(kd, lo, mid, x, y, z, best); + if (delta * delta < best) {kd_nearest(kd, mid + 1, hi, x, y, z, best);} + } else { + kd_nearest(kd, mid + 1, hi, x, y, z, best); + if (delta * delta < best) {kd_nearest(kd, lo, mid, x, y, z, best);} + } +} + +double SurfaceLodMesher::near_distance( + const std::vector & kd, double x, double y, double z, double reach) const +{ + // (beyond `reach` the driven-over ground changes nothing: infinity) + double best = reach * reach; + kd_nearest(kd, 0, kd.size(), x, y, z, best); + return best < reach * reach ? std::sqrt(best) : std::numeric_limits::infinity(); +} + +// --------------------------------------------------------------------------------------------- +// The policy + +int SurfaceLodMesher::level_of(std::uint32_t global) const +{ + return static_cast( + std::upper_bound(offset_.begin(), offset_.end(), static_cast(global)) - + offset_.begin()) - 1; +} + +// May the nodes of level k - 1 collapse into node c of level k, for this robot and ground? +bool SurfaceLodMesher::allowed_now(int k, std::uint32_t c, const Query & q) const +{ + const Level & lv = levels_[k]; + if (lv.flags[c] & kPinned) {return false;} + const LodParams & p = q.p; + const double s = lv.size, hyp = s * M_SQRT2; + const double dx = lv.x[c] - q.rx, dy = lv.y[c] - q.ry; + const double dz = cfg_.vertical_weight * (lv.z[c] - q.rz); + const double d_eff = effective_distance(std::sqrt(dx * dx + dy * dy + dz * dz), hyp); + const double err = lv.err[c]; + bool split = should_split_at(p, err, s, d_eff); + // (the smallest cells are always allowed: nothing to ask the driven-over ground) + if (!split && s > p.min_size && (!kd_known_.empty() || !kd_observed_.empty())) { + double d = d_eff; + for (const auto & [kd, pp] : {std::pair &, const LodParams &>{ + kd_known_, p}, {kd_observed_, q.po}}) + { + if (kd.empty()) {continue;} + // beyond this the ground cannot ask for more than the robot already does + // (known_effective_distance >= low_res_distance) + const double reach = kReach * hyp + pp.known_margin + + (pp.low_res_distance - pp.high_res_radius) / std::max(pp.known_falloff, 1e-6); + const double dk = near_distance(kd, lv.x[c], lv.y[c], lv.z[c], reach); + if (std::isfinite(dk)) {d = std::min(d, known_effective_distance(pp, dk, hyp));} + } + split = should_split_at(p, err, s, d); + } + return !split; +} + +bool SurfaceLodMesher::allowed_memo(int k, std::uint32_t c, const Query & q) const +{ + const std::size_t g = offset_[k] + c; + if (node_stamp_[g] != epoch_) { + node_stamp_[g] = epoch_; + node_memo_[g] = allowed_now(k, c, q) ? 1 : 0; + } + return node_memo_[g] != 0; +} + +// The decision of a robot infinitely far with no driven-over or observed ground. The policy only +// splits less with the distance (tolerance and size cap grow with it), so a node allowed now is +// allowed here: the far cut is never finer than a real one. +bool SurfaceLodMesher::static_allowed(int k, std::uint32_t c) const +{ + const Level & lv = levels_[k]; + if (lv.flags[c] & kPinned) {return false;} + const double s = lv.size; + if (!(s > far_min_)) {return true;} + return !(s > far_max_ || (far_growth_ <= 0.0 && lv.err[c] > far_base_)); +} + +std::uint32_t SurfaceLodMesher::static_leaf(std::uint32_t v) const +{ + std::uint32_t c = v; + std::size_t g = v; + for (std::size_t k = 1; k < levels_.size(); ++k) { + const std::uint32_t parent = levels_[k - 1].parent[c]; + if (!static_allowed(static_cast(k), parent)) {break;} + c = parent; + g = offset_[k] + c; + } + return static_cast(g); +} + +std::uint32_t SurfaceLodMesher::climb(std::uint32_t v, const Query & q) const +{ + std::uint32_t c = v; + std::size_t g = v; + for (std::size_t k = 1; k < levels_.size(); ++k) { + const std::uint32_t parent = levels_[k - 1].parent[c]; + if (!allowed_memo(static_cast(k), parent, q)) {break;} + c = parent; + g = offset_[k] + c; + } + return static_cast(g); +} + +void SurfaceLodMesher::next_epoch() const +{ + const std::size_t nodes = offset_.back(), V = levels_[0].x.size(); + const std::size_t nt = tri_.size() / 3, nf = far_slope_.size(); + if (node_stamp_.size() != nodes) { + node_stamp_.assign(nodes, 0); + node_memo_.assign(nodes, 0); + walk_stamp_.assign(nodes, 0); + slot_stamp_.assign(nodes, 0); + slot_.assign(nodes, 0); + known_stamp_.assign(nodes, 0); + vtx_stamp_.assign(V, 0); + cur_node_.assign(V, 0); + src_stamp_.assign(nt, 0); + } + if (far_stamp_.size() != nf) {far_stamp_.assign(nf, 0);} + if (++epoch_ == 0) { + for (auto * a : {&node_stamp_, &walk_stamp_, &slot_stamp_, &known_stamp_, &vtx_stamp_, + &far_stamp_, &src_stamp_}) + { + std::fill(a->begin(), a->end(), 0u); + } + epoch_ = 1; + } +} + +bool SurfaceLodMesher::far_cut_matches(const LodParams & p) const +{ + return far_built_ && far_min_ == p.min_size && far_max_ == p.max_size && + far_base_ == p.base_error && far_growth_ == p.error_growth; +} + +void SurfaceLodMesher::build_far_cut(const LodParams & p) const +{ + far_built_ = true; + far_min_ = p.min_size; + far_max_ = p.max_size; + far_base_ = p.base_error; + far_growth_ = p.error_growth; + const std::size_t V = levels_[0].x.size(), nt = tri_.size() / 3; + std::vector leaf(V); + for (std::uint32_t v = 0; v < V; ++v) { + leaf[v] = static_leaf(v); + } + far_tri_.clear(); + far_slope_.clear(); + far_size_.clear(); + far_of_src_.assign(nt, kNone); + std::unordered_map, std::uint32_t, TriHash> seen; + for (std::size_t t = 0; t < nt; ++t) { + const std::array g{leaf[tri_[3 * t]], leaf[tri_[3 * t + 1]], + leaf[tri_[3 * t + 2]]}; + if (g[0] == g[1] || g[1] == g[2] || g[0] == g[2]) {continue;} + auto key = g; + std::sort(key.begin(), key.end()); + const auto found = seen.emplace(key, static_cast(far_slope_.size())); + const std::uint32_t f = found.first->second; + far_of_src_[t] = f; + if (!found.second) { + far_slope_[f] = std::max(far_slope_[f], tri_slope_[t]); + continue; + } + far_tri_.insert(far_tri_.end(), g.begin(), g.end()); + far_slope_.push_back(tri_slope_[t]); + double size = 0.0; + for (const auto n : g) { + size = std::max(size, levels_[level_of(n)].size); + } + far_size_.push_back(static_cast(size)); + } + const std::size_t nf = far_slope_.size(); + far_src_start_.assign(nf + 1, 0); + for (std::size_t t = 0; t < nt; ++t) { + if (far_of_src_[t] != kNone) {++far_src_start_[far_of_src_[t] + 1];} + } + for (std::size_t f = 0; f < nf; ++f) { + far_src_start_[f + 1] += far_src_start_[f]; + } + far_src_.resize(far_src_start_[nf]); + std::vector fill(far_src_start_.begin(), far_src_start_.end() - 1); + for (std::size_t t = 0; t < nt; ++t) { + if (far_of_src_[t] != kNone) {far_src_[fill[far_of_src_[t]]++] = static_cast(t);} + } + far_tri_.shrink_to_fit(); + far_slope_.shrink_to_fit(); + far_size_.shrink_to_fit(); +} + +// --------------------------------------------------------------------------------------------- +// Extraction + +LodMesh SurfaceLodMesher::extract_reference( + double rx, double ry, double rz, const LodParams & p) const +{ + const std::size_t V = levels_[0].x.size(), nt = tri_.size() / 3; + build_kd(); + const Query q{p, observed_params(p), rx, ry, rz}; + next_epoch(); + const bool use_known = n_known_ > 0; + + // the node every vertex ends in: a cut of the tree + std::vector leaf(V); + for (std::uint32_t v = 0; v < V; ++v) { + leaf[v] = climb(v, q); + } + if (use_known) { + for (const auto v : known_list_) { + known_stamp_[leaf[v]] = epoch_; + } + } + LodMesh out; + const auto push_node = [&](std::uint32_t g) { + const int k = level_of(g); + const std::uint32_t c = g - static_cast(offset_[k]); + const Level & lv = levels_[k]; + out.x.push_back(lv.x[c]); + out.y.push_back(lv.y[c]); + out.z.push_back(lv.z[c]); + if (coloured_) { + out.r.push_back(lv.r[c]); + out.g.push_back(lv.g[c]); + out.b.push_back(lv.b[c]); + } + }; + std::unordered_map, std::uint32_t, TriHash> seen; + for (std::size_t t = 0; t < nt; ++t) { + const std::array g{leaf[tri_[3 * t]], leaf[tri_[3 * t + 1]], + leaf[tri_[3 * t + 2]]}; + if (g[0] == g[1] || g[1] == g[2] || g[0] == g[2]) {continue;} + auto key = g; + std::sort(key.begin(), key.end()); + const float slope = tri_slope_[t] / 100.0f; + const auto found = seen.emplace(key, static_cast(out.leg.size())); + if (!found.second) { + // the same triangle again (several source triangles end in it): the steepest one counts + out.slope[found.first->second] = std::max(out.slope[found.first->second], slope); + continue; + } + double leg = p.min_size; + for (const auto n : g) { + if (slot_stamp_[n] != epoch_) { + slot_stamp_[n] = epoch_; + slot_[n] = static_cast(out.x.size()); + push_node(n); + } + out.tri.push_back(slot_[n]); + leg = std::max(leg, levels_[level_of(n)].size); + } + out.leg.push_back(static_cast(leg)); + out.slope.push_back(slope); + out.traversed.push_back(use_known && known_stamp_[g[0]] == epoch_ && + known_stamp_[g[1]] == epoch_ && known_stamp_[g[2]] == epoch_); + } + return out; +} + +// The dynamic radius: a node (k, c) splits only where the effective distance to the robot, or to +// the driven-over / observed ground, is below split_radius_of(size, err). Outside those discs its +// decision is the far cut's. So only the vertices under the nodes inside them climb again, and +// only the source triangles of those vertices are mapped again; the rest of the mesh is the far +// cut's, as precomputed. The result is the reference's (see extract_reference and the tests). +LodMesh SurfaceLodMesher::extract(double rx, double ry, double rz, const LodParams & p) const +{ + if (!std::isfinite(rx) || !std::isfinite(ry) || !std::isfinite(rz) || !finite_params(p) || + p.error_growth < 0.0) + { + // (a policy that is not monotone in the distance has no far cut) + return extract_reference(rx, ry, rz, p); + } + if (!far_cut_matches(p)) {build_far_cut(p);} + build_kd(); + const Query q{p, observed_params(p), rx, ry, rz}; + next_epoch(); + const int L = static_cast(levels_.size()); + const bool use_known = n_known_ > 0; + + // 1. The nodes that may split now (near the robot, near the driven-over / observed ground) + // and do, among those the far cut lets merge + std::vector split; + const auto consider = [&](int k, std::uint32_t c) { + if (node_stamp_[offset_[k] + c] == epoch_) {return;} // already decided + if (static_allowed(k, c) && !allowed_memo(k, c, q)) { + split.push_back(static_cast(offset_[k] + c)); + } + }; + std::vector near_cells, visit; + for (int k = 1; k < L; ++k) { + const Level & lv = levels_[k]; + const double s = lv.size, hyp = s * M_SQRT2; + const double r = split_radius_of(s, lv.max_err, p); + if (r < 0.0) {continue;} + const CellIndex & cells = node_grid_[k]; + // robot: d_eff < r => 3-D distance < r + kReach * hyp => same in xy + const double rr = r + kReach * hyp; + cells.visit_disc(rx, ry, rr, [&](std::uint32_t c) { + if (std::hypot(lv.x[c] - rx, lv.y[c] - ry) <= rr) {consider(k, c);} + }); + for (const auto & [kd, pp] : {std::pair &, const LodParams &>{ + kd_known_, p}, {kd_observed_, q.po}}) + { + if (kd.empty()) {continue;} + // the ground: known_effective_distance(dk) < r, and dk within the reach the policy looks at + const double reach = kReach * hyp + pp.known_margin + + (pp.low_res_distance - pp.high_res_radius) / std::max(pp.known_falloff, 1e-6); + double kr = reach; + if (pp.known_falloff > 0.0) { + kr = std::min(reach, pp.known_margin + kReach * hyp + + std::max(0.0, r - pp.high_res_radius) / pp.known_falloff); + } + kr += 1e-3; + near_cells.clear(); + for (const auto & n : kd) { + near_cells.push_back(cell_key(n.x, n.y, cells.cell)); + } + std::sort(near_cells.begin(), near_cells.end()); + near_cells.erase(std::unique(near_cells.begin(), near_cells.end()), near_cells.end()); + const auto span = static_cast(std::ceil(std::min(kr, 1e12) / cells.cell)); + const double per = static_cast(2 * span + 1) * static_cast(2 * span + 1); + if (per * static_cast(near_cells.size()) > static_cast(lv.x.size())) { + // (cheaper to look at every node of the level) + for (std::uint32_t c = 0; c < lv.x.size(); ++c) { + consider(k, c); + } + continue; + } + visit.clear(); + for (const auto key : near_cells) { + const auto ix = key_x(key), iy = key_y(key); + for (std::int64_t j = iy - span; j <= iy + span; ++j) { + for (std::int64_t i = ix - span; i <= ix + span; ++i) { + visit.push_back(cell_key_of(i, j)); + } + } + } + std::sort(visit.begin(), visit.end()); + visit.erase(std::unique(visit.begin(), visit.end()), visit.end()); + for (const auto key : visit) { + cells.visit_cell(key, [&](std::uint32_t c) {consider(k, c);}); + } + } + } + + // 2. The vertices under them (through the nodes the far cut merges: the others stop below + // anyway), each subtree once, from the top + std::sort(split.begin(), split.end(), std::greater<>()); + std::vector affected, stack; + for (const auto top : split) { + if (walk_stamp_[top] == epoch_) {continue;} + walk_stamp_[top] = epoch_; + stack.push_back(top); + while (!stack.empty()) { + const std::uint32_t g = stack.back(); + stack.pop_back(); + const int k = level_of(g); + const Level & lv = levels_[k]; + const std::uint32_t c = g - static_cast(offset_[k]); + for (std::uint32_t i = lv.child_start[c]; i < lv.child_start[c + 1]; ++i) { + const std::uint32_t ch = lv.child[i]; + if (k == 1) { + if (vtx_stamp_[ch] != epoch_) {vtx_stamp_[ch] = epoch_; affected.push_back(ch);} + continue; + } + const auto gch = static_cast(offset_[k - 1] + ch); + if (walk_stamp_[gch] == epoch_ || !static_allowed(k - 1, ch)) {continue;} + walk_stamp_[gch] = epoch_; + stack.push_back(gch); + } + } + } + // they climb again; the others end where the far cut does + for (const auto v : affected) { + cur_node_[v] = climb(v, q); + } + const auto leaf = [&](std::uint32_t v) { + return vtx_stamp_[v] == epoch_ ? cur_node_[v] : static_leaf(v); + }; + if (use_known) { + for (const auto v : known_list_) { + known_stamp_[leaf(v)] = epoch_; + } + } + + // 3. Their source triangles are mapped again; the far triangles they ended in are remade from + // the source triangles left + std::vector remap_src, remade; + for (const auto v : affected) { + for (std::uint32_t i = vtx_tri_start_[v]; i < vtx_tri_start_[v + 1]; ++i) { + const std::uint32_t t = vtx_tri_[i]; + if (src_stamp_[t] == epoch_) {continue;} + src_stamp_[t] = epoch_; + remap_src.push_back(t); + const std::uint32_t f = far_of_src_[t]; + if (f != kNone && far_stamp_[f] != epoch_) {far_stamp_[f] = epoch_; remade.push_back(f);} + } + } + std::sort(remap_src.begin(), remap_src.end()); + std::sort(remade.begin(), remade.end()); + + LodMesh out; + const std::size_t nf = far_slope_.size(); + out.tri.reserve(3 * nf); + out.leg.reserve(nf); + out.slope.reserve(nf); + out.traversed.reserve(nf); + const auto node_vertex = [&](std::uint32_t g) { + if (slot_stamp_[g] != epoch_) { + slot_stamp_[g] = epoch_; + slot_[g] = static_cast(out.x.size()); + const int k = level_of(g); + const std::uint32_t c = g - static_cast(offset_[k]); + const Level & lv = levels_[k]; + out.x.push_back(lv.x[c]); + out.y.push_back(lv.y[c]); + out.z.push_back(lv.z[c]); + if (coloured_) { + out.r.push_back(lv.r[c]); + out.g.push_back(lv.g[c]); + out.b.push_back(lv.b[c]); + } + } + return slot_[g]; + }; + const auto emit = [&](const std::array & g, std::uint16_t slope, double size) { + for (const auto n : g) { + out.tri.push_back(node_vertex(n)); + } + out.leg.push_back(static_cast(std::max(p.min_size, size))); + out.slope.push_back(slope / 100.0f); + out.traversed.push_back(use_known && known_stamp_[g[0]] == epoch_ && + known_stamp_[g[1]] == epoch_ && known_stamp_[g[2]] == epoch_); + }; + // the far triangles untouched + for (std::size_t f = 0; f < nf; ++f) { + if (far_stamp_[f] == epoch_) {continue;} + emit({far_tri_[3 * f], far_tri_[3 * f + 1], far_tri_[3 * f + 2]}, far_slope_[f], far_size_[f]); + } + // the far triangles some of whose source triangles are left: those decide the slope, the + // first one the orientation + for (const auto f : remade) { + std::uint32_t first = kNone; + std::uint16_t slope = 0; + for (std::uint32_t i = far_src_start_[f]; i < far_src_start_[f + 1]; ++i) { + const std::uint32_t t = far_src_[i]; + if (src_stamp_[t] == epoch_) {continue;} + if (first == kNone) {first = t;} + slope = std::max(slope, tri_slope_[t]); + } + if (first == kNone) {continue;} + emit({static_leaf(tri_[3 * first]), static_leaf(tri_[3 * first + 1]), + static_leaf(tri_[3 * first + 2])}, slope, far_size_[f]); + } + // the source triangles mapped again: new triangles (each has a node no far triangle has) + std::unordered_map, std::uint32_t, TriHash> seen; + seen.reserve(remap_src.size()); + for (const auto t : remap_src) { + const std::array g{leaf(tri_[3 * t]), leaf(tri_[3 * t + 1]), + leaf(tri_[3 * t + 2])}; + if (g[0] == g[1] || g[1] == g[2] || g[0] == g[2]) {continue;} + auto key = g; + std::sort(key.begin(), key.end()); + const auto found = seen.emplace(key, static_cast(out.leg.size())); + if (!found.second) { + out.slope[found.first->second] = + std::max(out.slope[found.first->second], tri_slope_[t] / 100.0f); + continue; + } + double size = 0.0; + for (const auto n : g) { + size = std::max(size, levels_[level_of(n)].size); + } + emit(g, tri_slope_[t], size); + } + return out; +} + +std::size_t SurfaceLodMesher::memory_bytes() const +{ + std::size_t b = 0; + for (const auto & lv : levels_) { + b += (lv.x.capacity() + lv.y.capacity() + lv.z.capacity() + lv.err.capacity()) * + sizeof(float); + b += lv.r.capacity() + lv.g.capacity() + lv.b.capacity() + lv.flags.capacity(); + b += (lv.parent.capacity() + lv.child_start.capacity() + lv.child.capacity()) * + sizeof(std::uint32_t); + } + b += (tri_.capacity() + vtx_tri_start_.capacity() + vtx_tri_.capacity() + + known_list_.capacity() + observed_list_.capacity()) * sizeof(std::uint32_t); + b += tri_slope_.capacity() * sizeof(std::uint16_t) + known_.capacity() + observed_.capacity(); + b += offset_.capacity() * sizeof(std::size_t) + grid_.memory_bytes(); + for (const auto & g : node_grid_) { + b += g.memory_bytes(); + } + b += (kd_known_.capacity() + kd_observed_.capacity()) * sizeof(KdNode); + b += (far_tri_.capacity() + far_src_start_.capacity() + far_src_.capacity() + + far_of_src_.capacity()) * sizeof(std::uint32_t); + b += far_slope_.capacity() * sizeof(std::uint16_t) + far_size_.capacity() * sizeof(float); + b += (node_stamp_.capacity() + walk_stamp_.capacity() + slot_stamp_.capacity() + + slot_.capacity() + known_stamp_.capacity() + vtx_stamp_.capacity() + cur_node_.capacity() + + far_stamp_.capacity() + src_stamp_.capacity()) * sizeof(std::uint32_t) + + node_memo_.capacity(); + return b; +} + +} // namespace bigmap_maps_manager diff --git a/maps_managers/easynav_bigmap_maps_manager/test/data/dem_golden.bin b/maps_managers/easynav_bigmap_maps_manager/test/data/dem_golden.bin new file mode 100644 index 00000000..d9fffe25 Binary files /dev/null and b/maps_managers/easynav_bigmap_maps_manager/test/data/dem_golden.bin differ diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp new file mode 100644 index 00000000..9b38b166 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp @@ -0,0 +1,1284 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// The plugin as EasyNav runs it: parameters on a node, initialize(), update() with a robot pose +// and sensor points in the NavState. No simulator: a flat NavMap file made here, a box of points. + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/BigMapMapsManager.hpp" +#include "easynav_bigmap_maps_manager/dem_grid.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "navmap_core/Costs.hpp" +#include "navmap_core/NavMap.hpp" +#include "navmap_ros/navmap_io.hpp" +#include "navmap_ros_interfaces/msg/nav_map.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace +{ + +// update() is what EasyNav calls (through internal_update, which also throttles by freq) +class Plugin : public easynav::bigmap::BigMapMapsManager +{ +public: + using easynav::bigmap::BigMapMapsManager::update; +}; + +// A flat square NavMap of side 2*half m with cells of `res` m, saved to a temporary file +std::string flat_navmap(double half, double res, int hits_at_box = 0) +{ + ::navmap::NavMap nm; + const int n = static_cast(2 * half / res) + 1; + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + nm.positions.x.push_back(static_cast(-half + i * res)); + nm.positions.y.push_back(static_cast(-half + j * res)); + nm.positions.z.push_back(0.0f); + } + } + nm.surfaces.resize(1); + nm.surfaces[0].frame_id = "map"; + auto id = [n](int i, int j) {return static_cast(j * n + i);}; + for (int j = 0; j + 1 < n; ++j) { + for (int i = 0; i + 1 < n; ++i) { + for (const auto & t : {std::array{id(i, j), id(i + 1, j), id(i + 1, j + 1)}, + std::array{id(i, j), id(i + 1, j + 1), id(i, j + 1)}}) + { + ::navmap::NavCel c; + c.v[0] = t[0]; c.v[1] = t[1]; c.v[2] = t[2]; + nm.surfaces[0].navcels.push_back(static_cast<::navmap::NavCelId>(nm.navcels.size())); + nm.navcels.push_back(c); + } + } + } + nm.rebuild_geometry_accels(); + if (hits_at_box > 0) { // a saved map that counted obstacle hits where the test's box is + nm.add_layer("obstacle_hits", "", "", 0); + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto q = nm.navcel_centroid(c); + if (std::abs(q.x() - 3.0f) < 0.6f && std::abs(q.y()) < 0.6f) { + nm.layer_set("obstacle_hits", c, static_cast(hits_at_box)); + } + } + } + const auto path = (std::filesystem::temp_directory_path() / + ("bigmap_plugin_test_" + std::to_string(::getpid()) + ".navmap")).string(); + std::error_code ec; + if (!navmap_ros::io::save_to_file(nm, path, {}, &ec)) { + throw std::runtime_error("cannot write " + path + ": " + ec.message()); + } + return path; +} + +class BigMapPluginTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + easynav::RTTFBuffer::getInstance()->clear(); + map_file_ = flat_navmap(15.0, 0.5); + set("terrain.initial_navmap", map_file_); + set("lengths.cell_size.min", 0.5); + set("lengths.cell_size.max", 8.0); + set("lengths.radii.full_res", 6.0); + set("lengths.radii.low_res_start", 12.0); + set("lengths.radii.traversed_keep", 12.0); + set("lengths.radii.obstacle_clearance", 0.0); + set("dynamic_map.ground_fit.enabled", false); + set("dynamic_map.confirm_time", 0.05); + set("dynamic_map.forget_time", 0.4); + set("output.publish_markers", false); + set("filters", std::vector{"inflation"}); + set("inflation.plugin", + std::string("easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter")); + set("inflation.inflation_radius", 1.0); + set("inflation.cost_scaling_factor", 3.0); + } + + void TearDown() override {std::filesystem::remove(map_file_);} + + // As a parameter file gives them (overrides: the plugin also tells old names from new ones) + template + void set(const std::string & key, const T & value) + { + overrides_.emplace_back("bigmap." + key, rclcpp::ParameterValue(value)); + } + + std::shared_ptr make() + { + static int count = 0; + node_ = rclcpp_lifecycle::LifecycleNode::make_shared( + "bigmap_plugin_test_" + std::to_string(count++), + rclcpp::NodeOptions().parameter_overrides(overrides_)); + auto p = std::make_shared(); + p->initialize(node_, "bigmap"); + return p; + } + + void robot_at_origin() + { + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + o.pose.pose.orientation.w = 1.0; + nav_state_.set("robot_pose", o); + } + + // What the robot sees in its footprint frame: a box 0.6 x 0.6 x 1 m centred 3 m ahead, or nothing + void see(bool box) + { + easynav::PointPerception p; + p.frame_id = easynav::RTTFBuffer::getInstance()->get_tf_info().robot_footprint_frame; + p.stamp = node_->now(); + p.valid = true; + if (box) { + for (float x = 2.7f; x <= 3.3f; x += 0.05f) { + for (float y = -0.3f; y <= 0.3f; y += 0.05f) { + for (float z = 0.05f; z <= 1.0f; z += 0.05f) {p.data.push_back(pcl::PointXYZ(x, y, z));} + } + } + } else { + p.data.push_back(pcl::PointXYZ(0.5f, 0.0f, 0.0f)); // ground only + } + nav_state_.set("scan", p); + } + + // Runs update() for `seconds` of wall time (the plugin's clock), seeing the box or not + void run(Plugin & p, bool box, double seconds) + { + const auto end = std::chrono::steady_clock::now() + std::chrono::duration(seconds); + while (std::chrono::steady_clock::now() < end) { + see(box); + p.update(nav_state_); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + } + + // NavCels whose centroid is within half a metre of the box's centre: their values in `layer` + std::vector at_box(const std::string & layer) const + { + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + std::vector out; + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto q = nm.navcel_centroid(c); + if (std::abs(q.x() - 3.0f) < 0.5f && std::abs(q.y()) < 0.5f) { + out.push_back(nm.layer_get(layer, c, 0)); + } + } + return out; + } + + static std::size_t count_at_least(const std::vector & v, std::uint8_t value) + { + std::size_t n = 0; + for (const auto x : v) {n += x >= value;} + return n; + } + + std::vector overrides_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + easynav::NavState nav_state_; + std::string map_file_; + easynav::NavState * nav_state_ref_{nullptr}; +}; + +} // namespace + +TEST_F(BigMapPluginTest, PublishesTheMapWithItsLayersAndTheInflation) +{ + auto p = make(); + robot_at_origin(); + see(false); + p->update(nav_state_); + ASSERT_TRUE(nav_state_.has("map.navmap")); + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + EXPECT_GT(nm.navcels.size(), 100u); + for (const char * layer : {"occupancy", "obstacles", "obstacle_hits", "traversed", "elevation", + "lod_size", "inflated_obstacles"}) + { + EXPECT_TRUE(nm.has_layer(layer)) << layer; + } + // flat ground: nothing is an obstacle + EXPECT_EQ(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); +} + +TEST_F(BigMapPluginTest, AnObstacleCountsAfterMinTimeAndStaysOnceFixed) +{ + set("obstacles.memory.min_time", 0.3); + set("obstacles.memory.max_hits", 2); + auto p = make(); + robot_at_origin(); + run(*p, true, 0.15); // seen, but for less than min_time: not an obstacle yet + EXPECT_EQ(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + run(*p, true, 0.9); // > 2 x min_time: two hits, fixed + const auto hits = at_box("obstacle_hits"); + ASSERT_FALSE(hits.empty()); + EXPECT_GT(count_at_least(hits, 2), 0u); + EXPECT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + // the box goes: a fixed obstacle stays, after its detections are forgotten (forget_time 0.4 s) + run(*p, false, 1.0); + EXPECT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + EXPECT_GT(count_at_least(at_box("obstacle_hits"), 2), 0u); +} + +TEST_F(BigMapPluginTest, WithoutMemoryAnObstacleGoesWhenItIsNoLongerSeen) +{ + set("obstacles.memory.enabled", false); + set("dynamic_map.enabled", true); + auto p = make(); + robot_at_origin(); + run(*p, true, 0.5); + EXPECT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + EXPECT_EQ(count_at_least(at_box("obstacle_hits"), 1), 0u); // nothing counted + run(*p, false, 1.0); + EXPECT_EQ(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); +} + +TEST_F(BigMapPluginTest, NoSensorObstaclesWhenNeitherTheDynamicMapNorTheMemoryIsOn) +{ + set("obstacles.memory.enabled", false); + set("dynamic_map.enabled", false); + auto p = make(); + robot_at_origin(); + run(*p, true, 0.6); + EXPECT_EQ(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); +} + +TEST_F(BigMapPluginTest, TheFilterDetectorWithoutAnObstacleFilterDoesNotStart) +{ + set("obstacles.detector", std::string("navmap_filter")); + EXPECT_THROW(make(), std::invalid_argument); +} + +TEST_F(BigMapPluginTest, TheOldDetectorNameIsStillAccepted) +{ + set("obstacles.source", std::string("dynamic")); + EXPECT_NO_THROW(make()); +} + +TEST_F(BigMapPluginTest, SwitchingTheMemoryOffAtRuntimeReleasesTheFixedObstacles) +{ + // what a recovery does to let the obstacles go (dynamic_map off: nothing else analyses sensors) + set("obstacles.memory.min_time", 0.3); + set("obstacles.memory.max_hits", 2); + auto p = make(); + robot_at_origin(); + run(*p, true, 0.9); + run(*p, false, 0.6); // the box went; fixed: still blocked + ASSERT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + node_->set_parameter(rclcpp::Parameter("bigmap.obstacles.memory.enabled", false)); + run(*p, false, 0.1); + EXPECT_EQ(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + EXPECT_EQ(count_at_least(at_box("obstacle_hits"), 1), 0u); // not shown: not an obstacle now + // switched on again: the memory kept its counts, the obstacle is back + node_->set_parameter(rclcpp::Parameter("bigmap.obstacles.memory.enabled", true)); + run(*p, false, 0.3); + EXPECT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); +} + +TEST_F(BigMapPluginTest, TheHitsOfASavedMapComeBackAsObstacles) +{ + set("obstacles.memory.max_hits", 3); + std::filesystem::remove(map_file_); + map_file_ = flat_navmap(15.0, 0.5, 3); // fixed where the box will be + set("terrain.initial_navmap", map_file_); + auto p = make(); + robot_at_origin(); + run(*p, false, 0.3); // nothing is seen there, yet it is an obstacle + EXPECT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + EXPECT_GT(count_at_least(at_box("obstacle_hits"), 3), 0u); +} + +TEST_F(BigMapPluginTest, TheClearServiceForgetsTheFixedObstacles) +{ + set("obstacles.memory.min_time", 0.3); + set("obstacles.memory.max_hits", 2); + auto p = make(); + robot_at_origin(); + run(*p, true, 0.9); + run(*p, false, 0.6); // the box went; fixed: still blocked + ASSERT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + + auto client_node = rclcpp::Node::make_shared("bigmap_plugin_test_client"); + auto client = client_node->create_client( + std::string(node_->get_fully_qualified_name()) + "/bigmap/clear_obstacles"); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(client_node); + exec.add_node(node_->get_node_base_interface()); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(5))); + auto result = client->async_send_request(std::make_shared()); + ASSERT_EQ( + exec.spin_until_future_complete(result, std::chrono::seconds(5)), + rclcpp::FutureReturnCode::SUCCESS); + EXPECT_TRUE(result.get()->success); + + run(*p, false, 0.2); // update() does it in its next cycle + EXPECT_EQ(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + EXPECT_EQ(count_at_least(at_box("obstacle_hits"), 1), 0u); + run(*p, false, 0.6); // and it does not come back + EXPECT_EQ(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); +} + +TEST_F(BigMapPluginTest, TheInflationSeesTheObstaclesOfTheSameCycleAlsoAfterARemesh) +{ + set("obstacles.memory.min_time", 0.3); + set("obstacles.memory.max_hits", 1); + set("lengths.remesh_step", 0.5); + auto p = make(); + robot_at_origin(); + run(*p, true, 0.6); + ASSERT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + // the robot moves 0.6 m: re-mesh (new NavMap in the NavState) in this very update() + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + o.pose.pose.position.x = 0.6; + o.pose.pose.orientation.w = 1.0; + nav_state_.set("robot_pose", o); + see(false); + p->update(nav_state_); + // in the same cycle: the fixed obstacle is lethal and the inflation was built on it + EXPECT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + EXPECT_GT(count_at_least(at_box("inflated_obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); +} + +namespace +{ +// The largest NavCel (lod_size) whose centroid is between r0 and r1 m from (cx, 0) +float coarsest_in_ring(const easynav::NavState & ns, double cx, double r0, double r1) +{ + const auto & nm = ns.get<::navmap::NavMap>("map.navmap"); + float worst = 0.0f; + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto q = nm.navcel_centroid(c); + const double d = std::hypot(q.x() - cx, q.y()); + if (d >= r0 && d <= r1) {worst = std::max(worst, nm.layer_get("lod_size", c, 0.0f));} + } + return worst; +} + +void robot_at(easynav::NavState & ns, double x) +{ + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + o.pose.pose.position.x = x; + o.pose.pose.orientation.w = 1.0; + ns.set("robot_pose", o); +} +} // namespace + +TEST_F(BigMapPluginTest, WithAGoalAndNoPathTheFullResolutionCircleGrowsUntilTheGoalIsGone) +{ + set("recovery.no_path_cycles", int64_t{2}); + set("recovery.max_full_res_factor", 2.0); + auto p = make(); + robot_at_origin(); + see(false); + p->update(nav_state_); + const float before = coarsest_in_ring(nav_state_, 0.0, 7.0, 11.0); + ASSERT_GT(before, 0.5f); // beyond the 6 m of full resolution: coarser + + nav_msgs::msg::Goals goals; + goals.goals.emplace_back(); + goals.goals.back().pose.position.x = 14.0; + nav_state_.set("goals", goals); + nav_state_.set("path", nav_msgs::msg::Path()); + p->update(nav_state_); + EXPECT_EQ(coarsest_in_ring(nav_state_, 0.0, 7.0, 11.0), before); // one cycle is not enough + p->update(nav_state_); + const float during = coarsest_in_ring(nav_state_, 0.0, 7.0, 11.0); + EXPECT_LT(during, before); // full resolution within 12 m now + + // a path: nothing changes (and no further growth: the cap is 2 x 6 m) + nav_msgs::msg::Path path; + path.poses.emplace_back(); + nav_state_.set("path", path); + for (int i = 0; i < 4; ++i) { + p->update(nav_state_); + } + EXPECT_EQ(coarsest_in_ring(nav_state_, 0.0, 7.0, 11.0), during); + + // the goal is gone: back to normal + nav_state_.set("goals", nav_msgs::msg::Goals()); + p->update(nav_state_); + EXPECT_EQ(coarsest_in_ring(nav_state_, 0.0, 7.0, 11.0), before); +} + +TEST_F(BigMapPluginTest, TheRecoveryCanBeSwitchedOff) +{ + set("recovery.enabled", false); + set("recovery.no_path_cycles", int64_t{1}); + auto p = make(); + robot_at_origin(); + see(false); + p->update(nav_state_); + const float before = coarsest_in_ring(nav_state_, 0.0, 7.0, 11.0); + nav_msgs::msg::Goals goals; + goals.goals.emplace_back(); + nav_state_.set("goals", goals); + for (int i = 0; i < 4; ++i) { + p->update(nav_state_); + } + EXPECT_EQ(coarsest_in_ring(nav_state_, 0.0, 7.0, 11.0), before); +} + +TEST_F(BigMapPluginTest, TheReMeshInAWorkerGivesTheSameMapAsInTheCycle) +{ + auto sync = make(); + const auto sync_node = node_; // (the plugin does not own its node) + set("lod.async_remesh", true); + auto async = make(); + easynav::NavState ns_sync, ns_async; + for (auto * ns : {&ns_sync, &ns_async}) { + robot_at(*ns, 0.0); + easynav::PointPerception ground; // as see(false) + ground.frame_id = easynav::RTTFBuffer::getInstance()->get_tf_info().robot_footprint_frame; + ground.stamp = node_->now(); + ground.valid = true; + ground.data.push_back(pcl::PointXYZ(0.5f, 0.0f, 0.0f)); + ns->set("scan", ground); + } + sync->update(ns_sync); + async->update(ns_async); // the first mesh is made in the cycle + ASSERT_TRUE(ns_async.has("map.navmap")); + EXPECT_EQ(ns_async.get<::navmap::NavMap>("map.navmap").navcels.size(), + ns_sync.get<::navmap::NavMap>("map.navmap").navcels.size()); + + for (const double x : {3.0, 6.0, 9.0}) { + robot_at(ns_sync, x); + robot_at(ns_async, x); + sync->update(ns_sync); + const auto before = ns_async.get<::navmap::NavMap>("map.navmap").navcels.size(); + async->update(ns_async); // asks for it; the previous map stays meanwhile + EXPECT_EQ(ns_async.get<::navmap::NavMap>("map.navmap").navcels.size(), before); + const auto end = std::chrono::steady_clock::now() + std::chrono::seconds(5); + std::size_t now = before; + while (now == before && std::chrono::steady_clock::now() < end) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + async->update(ns_async); + now = ns_async.get<::navmap::NavMap>("map.navmap").navcels.size(); + } + EXPECT_EQ(now, ns_sync.get<::navmap::NavMap>("map.navmap").navcels.size()) << x; + EXPECT_EQ(coarsest_in_ring(ns_async, x, 7.0, 11.0), coarsest_in_ring(ns_sync, x, 7.0, 11.0)); + } + // destroyed with nothing running, or waiting for it: never while the worker reads the plugin + robot_at(ns_async, 12.0); + async->update(ns_async); + async.reset(); +} + +TEST_F(BigMapPluginTest, ANonFinitePoseIsNoPose) +{ + auto p = make(); + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + o.pose.pose.position.x = std::nan(""); + o.pose.pose.orientation.w = 1.0; + nav_state_.set("robot_pose", o); + see(false); + p->update(nav_state_); + EXPECT_FALSE(nav_state_.has("map.navmap")); // nothing meshed around NaN + robot_at_origin(); + p->update(nav_state_); + ASSERT_TRUE(nav_state_.has("map.navmap")); + EXPECT_GT(nav_state_.get<::navmap::NavMap>("map.navmap").navcels.size(), 100u); +} + +TEST_F(BigMapPluginTest, InvalidValuesStopTheStartWithAMessage) +{ + set("dynamic_map.ground_fit.min_cells", 0); + EXPECT_THROW(make(), std::invalid_argument); +} + +TEST_F(BigMapPluginTest, TheMapShownHasTheInflationOfTheCycleThatRemovedItsNavCels) +{ + set("obstacles.memory.min_time", 0.3); + set("obstacles.memory.max_hits", 200); + set("obstacles.removal.enabled", true); + auto p = make(); + auto listener = std::make_shared("bigmap_plugin_test_listener"); + std::vector maps; + auto sub = listener->create_subscription( + "/" + std::string(node_->get_name()) + "/bigmap/map", rclcpp::QoS(10).reliable(), + [&maps](navmap_ros_interfaces::msg::NavMap::ConstSharedPtr m) {maps.push_back(*m);}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(listener); + robot_at_origin(); + // the box is seen for half a second and then gone: its NavCels are removed and then given back + const auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::milliseconds(1500)) { + see(std::chrono::steady_clock::now() - start < std::chrono::milliseconds(500)); + p->update(nav_state_); + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + executor.spin_some(); + const std::size_t full = nav_state_.get<::navmap::NavMap>("map.navmap").navcels.size(); + auto costly = [](const navmap_ros_interfaces::msg::NavMap & m) -> std::size_t { + for (const auto & l : m.layers) { + if (l.name == "inflated_obstacles") { + return static_cast( + std::count_if(l.data_u8.begin(), l.data_u8.end(), [](auto v) {return v > 0;})); + } + } + return SIZE_MAX; // the map of the first cycle, before there is an inflation + }; + // what the map's border alone costs: the smallest inflation of all the maps shown + std::size_t border = SIZE_MAX; + for (const auto & m : maps) { + border = std::min(border, costly(m)); + } + std::size_t with_holes = 0, without_holes = 0; + for (const auto & m : maps) { + if (costly(m) == SIZE_MAX) {continue;} + // the removed NavCels are lethal: the inflation of the very same map costs around them. A map + // without them costs the border only, not the inflation of an obstacle just given back + const bool holes = m.navcels_v0.size() < full; + (holes ? with_holes : without_holes)++; + EXPECT_EQ(costly(m) > border, holes) << (holes ? "with" : "without") << " removed NavCels"; + } + EXPECT_GT(with_holes, 0u); + EXPECT_GT(without_holes, 0u); +} + +TEST_F(BigMapPluginTest, HostileInputsNeverThrowOrLeaveTheMapInvalid) +{ + set("obstacles.memory.min_time", 0.05); + set("obstacles.memory.max_hits", 2); + set("lengths.remesh_step", 0.5); + auto p = make(); + std::mt19937 rng(777); // fixed seed: a failure is reproducible + std::uniform_real_distribution unit(-1.0, 1.0); + const double nan = std::numeric_limits::quiet_NaN(); + const double inf = std::numeric_limits::infinity(); + const double poses[][7] = { + {0, 0, 0, 0, 0, 0, 0}, // a zero quaternion + {1e9, 0, 0, 0, 0, 0, 1}, // far outside the map + {-1e12, 1e12, 0, 0, 0, 0, 1}, + {0, 0, 1e9, 0, 0, 0, 1}, // absurd height + {0, 0, 0, 5, 5, 5, 5}, // not a unit quaternion + {nan, 0, 0, 0, 0, 0, 1}, {0, inf, 0, 0, 0, 0, 1}, {0, 0, 0, nan, 0, 0, 1}, + }; + for (int k = 0; k < 160; ++k) { + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + if (k < 8 * 4) { + const auto & q = poses[k % 8]; + o.pose.pose.position.x = q[0]; o.pose.pose.position.y = q[1]; o.pose.pose.position.z = q[2]; + o.pose.pose.orientation.x = q[3]; o.pose.pose.orientation.y = q[4]; + o.pose.pose.orientation.z = q[5]; o.pose.pose.orientation.w = q[6]; + } else { + o.pose.pose.position.x = 12.0 * unit(rng); o.pose.pose.position.y = 12.0 * unit(rng); + o.pose.pose.orientation.w = 1.0; + } + nav_state_.set("robot_pose", o); + easynav::PointPerception pc; + pc.frame_id = easynav::RTTFBuffer::getInstance()->get_tf_info().robot_footprint_frame; + pc.stamp = node_->now(); + pc.valid = true; + const int n = static_cast(rng() % 400); // sometimes none at all + for (int j = 0; j < n; ++j) { + float v[3] = {static_cast(8 * unit(rng)), static_cast(8 * unit(rng)), + static_cast(2 * unit(rng))}; + if (rng() % 17 == 0) {v[rng() % 3] = std::numeric_limits::quiet_NaN();} + if (rng() % 23 == 0) {v[rng() % 3] = std::numeric_limits::infinity();} + if (rng() % 29 == 0) {v[rng() % 3] = 3.0e38f;} + pc.data.push_back(pcl::PointXYZ(v[0], v[1], v[2])); + } + nav_state_.set("scan", pc); + ASSERT_NO_THROW(p->update(nav_state_)) << "cycle " << k; + if (nav_state_.has("map.navmap")) { + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + for (const auto & name : nm.list_layers()) { + ASSERT_EQ(nm.layers.get(name)->size(), nm.navcels.size()) << name << " cycle " << k; + } + for (const auto & v : nm.positions.z) { + ASSERT_TRUE(std::isfinite(v)) << "cycle " << k; + } + } + } +} + +namespace +{ +// A hilly DEM file of `side` x `side` nodes of 1 m: what navmap_gis_tool --dem-out would write. +// `hole` (i0, j0, i1, j1 cells, inclusive): a building, cells without ground (--dem-holes-mesh) +std::string hilly_dem_file(int side, std::array hole = {-1, -1, -1, -1}) +{ + bigmap_maps_manager::DemGrid g; + g.side = side; + g.resolution = 1.0; + g.data_resolution = 1.0; + g.radius = (side - 1) / 2.0; + g.origin_lat = 40.4; + g.origin_lon = -3.7; + g.reference_altitude = 650.0; + g.source = bigmap_maps_manager::DemGrid::kCopernicus; + g.z.resize(static_cast(side) * side); + for (int j = 0; j < side; ++j) { + for (int i = 0; i < side; ++i) { + g.z[static_cast(j) * side + i] = + static_cast(2.0 * std::sin(0.2 * i) * std::cos(0.15 * j)); + } + } + if (hole[0] >= 0) { + const int nc = side - 1; + g.cell_valid.assign(static_cast(nc) * nc, 1); + for (int j = hole[1]; j <= hole[3]; ++j) { + for (int i = hole[0]; i <= hole[2]; ++i) { + g.cell_valid[static_cast(j) * nc + i] = 0; + } + } + } + const auto path = (std::filesystem::temp_directory_path() / + ("bigmap_plugin_test_dem_" + std::to_string(::getpid()) + ".bin")).string(); + if (!g.save(path)) {throw std::runtime_error("cannot write " + path);} + return path; +} +} // namespace + +TEST_F(BigMapPluginTest, StartsFromADemFileAndPublishesItsTerrainAtTheGpsOrigin) +{ + const auto dem = hilly_dem_file(65); + overrides_.erase( + std::remove_if( + overrides_.begin(), overrides_.end(), + [](const rclcpp::Parameter & q) {return q.get_name() == "bigmap.terrain.initial_navmap";}), + overrides_.end()); + set("terrain.dem_file", dem); + auto p = make(); + robot_at_origin(); + see(false); + p->update(nav_state_); + std::filesystem::remove(dem); + ASSERT_TRUE(nav_state_.has("map.navmap")); + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + EXPECT_GT(nm.navcels.size(), 50u); + float lo = 1e9f, hi = -1e9f; + for (const auto z : nm.positions.z) { + lo = std::min(lo, z); hi = std::max(hi, z); + } + EXPECT_GT(hi - lo, 1.0f) << "the hills of the DEM are in the mesh"; + EXPECT_LT(hi, 3.0f); + EXPECT_GT(lo, -3.0f); +} + +TEST_F(BigMapPluginTest, TheHolesOfADemFileAreHolesOfTheMap) +{ + // a 6 x 6 m building at x 8..14, y -2..4 (cells 40..45, 30..35 of a 65-node grid centred on 0) + const auto dem = hilly_dem_file(65, {40, 30, 45, 35}); + overrides_.erase( + std::remove_if( + overrides_.begin(), overrides_.end(), + [](const rclcpp::Parameter & q) {return q.get_name() == "bigmap.terrain.initial_navmap";}), + overrides_.end()); + set("terrain.dem_file", dem); + auto p = make(); + robot_at_origin(); + see(false); + p->update(nav_state_); + std::filesystem::remove(dem); + ASSERT_TRUE(nav_state_.has("map.navmap")); + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + // whether (x, y) is in the building grown by `m` metres on every side + const auto in_building = [](double x, double y, double m) { + return std::abs(x - 11.0) < 3.0 + m && std::abs(y - 1.0) < 3.0 + m; + }; + std::size_t inside = 0, around = 0; + for (std::size_t t = 0; t < nm.navcels.size(); ++t) { + double cx = 0.0, cy = 0.0; + for (int k = 0; k < 3; ++k) { + cx += nm.positions.x[nm.navcels[t].v[k]] / 3.0; + cy += nm.positions.y[nm.navcels[t].v[k]] / 3.0; + } + inside += in_building(cx, cy, 0.0); + around += in_building(cx, cy, 3.0) && !in_building(cx, cy, 0.0); + } + EXPECT_EQ(inside, 0u) << "no ground on the building"; + EXPECT_GT(around, 20u) << "the ground around it is there"; +} + +TEST_F(BigMapPluginTest, AMissingDemFileStopsTheStartWithAMessage) +{ + overrides_.erase( + std::remove_if( + overrides_.begin(), overrides_.end(), + [](const rclcpp::Parameter & q) {return q.get_name() == "bigmap.terrain.initial_navmap";}), + overrides_.end()); + set("terrain.dem_file", std::string("/nonexistent/dem.bin")); + set("terrain.dem_wait_timeout", 0.2); + EXPECT_THROW(make(), std::runtime_error); +} + +TEST_F(BigMapPluginTest, TheSaveServicesWriteAMapThatLoadsBack) +{ + const auto dir = std::filesystem::temp_directory_path() / + ("bigmap_plugin_test_saved_" + std::to_string(::getpid())); + set("output.save_directory", dir.string()); + auto p = make(); + auto client_node = std::make_shared("bigmap_plugin_test_client"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(client_node); + executor.add_node(node_->get_node_base_interface()); // serves the plugin's services + robot_at_origin(); + see(false); + p->update(nav_state_); + const std::string base = "/" + std::string(node_->get_name()) + "/bigmap/"; + auto call = [&](const std::string & service) { + auto client = client_node->create_client(base + service); + auto future = client->async_send_request(std::make_shared()); + const auto end = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (future.wait_for(std::chrono::milliseconds(0)) != std::future_status::ready && + std::chrono::steady_clock::now() < end) + { + executor.spin_some(); + see(false); + p->update(nav_state_); // the save is done by update(), in its next cycle + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + return future.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready ? + future.get() : nullptr; + }; + const auto traversed = call("save_traversed"); + ASSERT_NE(traversed, nullptr); + EXPECT_TRUE(traversed->success) << traversed->message; + const auto all = call("save_map"); + ASSERT_NE(all, nullptr); + EXPECT_TRUE(all->success) << all->message; + // a second save within the same second must not overwrite the first one + const auto again = call("save_map"); + ASSERT_NE(again, nullptr); + EXPECT_TRUE(again->success) << again->message; + for (int k = 0; k < 10; ++k) { + see(false); p->update(nav_state_); + } + std::size_t cells_all = 0, cells_traversed = 0, files = 0; + for (const auto & e : std::filesystem::directory_iterator(dir)) { + if (e.path().extension() != ".navmap") {continue;} + ++files; + ::navmap::NavMap loaded; + std::error_code ec; + ASSERT_TRUE(navmap_ros::io::load_from_file(e.path().string(), loaded, &ec)) << ec.message(); + const bool only_traversed = e.path().filename().string().find("traversed") != std::string::npos; + (only_traversed ? cells_traversed : cells_all) = loaded.navcels.size(); + } + EXPECT_EQ(files, 3u) << "traversed + two full maps"; + EXPECT_GT(cells_traversed, 0u); + EXPECT_GT(cells_all, cells_traversed) << "the traversed map is only the ground driven over"; + std::filesystem::remove_all(dir); +} + +TEST_F(BigMapPluginTest, TheMarkersAreWhatTheLodPolicyDraws) +{ + set("output.publish_markers", true); + auto p = make(); + auto listener = std::make_shared("bigmap_plugin_test_marker_listener"); + std::vector got; + auto sub = listener->create_subscription( + "/" + std::string(node_->get_name()) + "/bigmap/markers", + rclcpp::QoS(1).transient_local().reliable(), + [&got](visualization_msgs::msg::MarkerArray::ConstSharedPtr m) {got.push_back(*m);}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(listener); + robot_at_origin(); + see(false); + p->update(nav_state_); + for (int k = 0; k < 20; ++k) { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ASSERT_FALSE(got.empty()); + std::size_t triangles = 0; + for (const auto & m : got.back().markers) { + for (const auto & pt : m.points) { + ASSERT_TRUE(std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)); + } + if (m.type == visualization_msgs::msg::Marker::TRIANGLE_LIST) { + triangles += m.points.size() / 3; + } + } + EXPECT_GT(triangles, 100u); +} + +TEST_F(BigMapPluginTest, + ASubscriberToTheLayerUpdatesGetsTheWholeInflationOnceAndNothingWhileItDoesNotChange) +{ + set("obstacles.removal.enabled", false); // with removal the layers travel inside the map + set("obstacles.memory.enabled", false); + set("dynamic_map.enabled", true); + auto p = make(); + auto listener = std::make_shared("bigmap_plugin_test_update_listener"); + std::vector got; + auto sub = listener->create_subscription( + "/" + std::string(node_->get_name()) + "/bigmap/map_updates", rclcpp::QoS(100), + [&got](navmap_ros_interfaces::msg::NavMapLayer::ConstSharedPtr m) {got.push_back(*m);}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(listener); + robot_at_origin(); + // let discovery happen: the plugin only publishes updates when it has a subscriber + for (int k = 0; k < 30; ++k) { + see(false); + p->update(nav_state_); + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + ASSERT_FALSE(got.empty()); + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + EXPECT_EQ(got.front().name, "inflated_obstacles"); + EXPECT_TRUE(got.front().indices.empty()) << "the first one is the whole layer"; + EXPECT_EQ(got.front().data_u8.size(), nm.navcels.size()); + const auto before = got.size(); + run(*p, true, 0.6); // nothing reaches `obstacles` without removal: the layer does not change + for (int k = 0; k < 10; ++k) { + executor.spin_some(); + } + EXPECT_EQ(got.size(), before) << "an unchanged layer is not sent again"; +} + +TEST_F(BigMapPluginTest, LoweringViewMaxZAtRuntimeRepublishesTheMapWithoutTheHighNavCels) +{ + set("output.view_max_z", 1e9); + auto p = make(); + auto listener = std::make_shared("bigmap_plugin_test_view_listener"); + std::vector sizes; + auto sub = listener->create_subscription( + "/" + std::string(node_->get_name()) + "/bigmap/map", rclcpp::QoS(10).reliable(), + [&sizes](navmap_ros_interfaces::msg::NavMap::ConstSharedPtr m) { + sizes.push_back(m->navcels_v0.size()); + }); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(listener); + robot_at_origin(); + see(false); + p->update(nav_state_); + for (int k = 0; k < 10; ++k) { + executor.spin_some(); + } + ASSERT_FALSE(sizes.empty()); + const auto full = sizes.back(); + // the ground is flat at z = 0: everything is above -1 + node_->set_parameter(rclcpp::Parameter("bigmap.output.view_max_z", -1.0)); + see(false); + p->update(nav_state_); + for (int k = 0; k < 10; ++k) { + executor.spin_some(); + } + EXPECT_LT(sizes.back(), full); + EXPECT_EQ(sizes.back(), 0u); +} + +namespace +{ +// Two flat floors of side 2*half m over each other (`gap` m apart, no ramp), cells of `res` m: +// what the hierarchy engine is for. The lower floor is at z = 0. +std::string two_floors_navmap(double half, double res, double gap) +{ + ::navmap::NavMap nm; + const int n = static_cast(2 * half / res) + 1; + for (int floor = 0; floor < 2; ++floor) { + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + nm.positions.x.push_back(static_cast(-half + i * res)); + nm.positions.y.push_back(static_cast(-half + j * res)); + nm.positions.z.push_back(static_cast(floor * gap)); + } + } + } + nm.surfaces.resize(1); + nm.surfaces[0].frame_id = "map"; + for (int floor = 0; floor < 2; ++floor) { + auto id = [n, floor](int i, int j) { + return static_cast(floor * n * n + j * n + i); + }; + for (int j = 0; j + 1 < n; ++j) { + for (int i = 0; i + 1 < n; ++i) { + for (const auto & t : {std::array{id(i, j), id(i + 1, j), + id(i + 1, j + 1)}, std::array{id(i, j), id(i + 1, j + 1), + id(i, j + 1)}}) + { + ::navmap::NavCel c; + c.v[0] = t[0]; c.v[1] = t[1]; c.v[2] = t[2]; + nm.surfaces[0].navcels.push_back(static_cast<::navmap::NavCelId>(nm.navcels.size())); + nm.navcels.push_back(c); + } + } + } + } + nm.rebuild_geometry_accels(); + const auto path = (std::filesystem::temp_directory_path() / + ("bigmap_plugin_test_floors_" + std::to_string(::getpid()) + ".navmap")).string(); + std::error_code ec; + if (!navmap_ros::io::save_to_file(nm, path, {}, &ec)) { + throw std::runtime_error("cannot write " + path + ": " + ec.message()); + } + return path; +} +} // namespace + +// The hierarchy engine (multilevel.enabled) through the plugin: two floors over each other, the +// robot on the lower one. The box it sees is an obstacle on its own floor only, the traversed +// ground is marked on its floor only, and a change of floor (z) re-meshes. +TEST_F(BigMapPluginTest, TheHierarchyEngineKeepsTheFloorsApartForObstaclesAndTraversedGround) +{ + std::filesystem::remove(map_file_); + map_file_ = two_floors_navmap(15.0, 0.5, 3.0); + set("terrain.initial_navmap", map_file_); + set("multilevel.enabled", true); + set("multilevel.vertical_tolerance", 1.0); + set("obstacles.memory.min_time", 0.3); + set("obstacles.memory.max_hits", 2); + auto p = make(); + robot_at_origin(); + run(*p, true, 0.9); // two hits: fixed on the lower floor + ASSERT_TRUE(nav_state_.has("map.navmap")); + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + std::size_t lower = 0, upper = 0, lethal_lower = 0, lethal_upper = 0, traversed_lower = 0, + traversed_upper = 0; + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto q = nm.navcel_centroid(c); + const bool up = q.z() > 1.5f; + (up ? upper : lower)++; + if (std::abs(q.x() - 3.0f) < 0.5f && std::abs(q.y()) < 0.5f && + nm.layer_get("obstacles", c, 0) == ::navmap::LETHAL_OBSTACLE) + { + (up ? lethal_upper : lethal_lower)++; + } + if (nm.layer_get("traversed", c, 0) == 255) { + (up ? traversed_upper : traversed_lower)++; + } + } + EXPECT_GT(lower, 100u); + EXPECT_GT(upper, 100u) << "both floors are in the map (the robot's and the one above)"; + EXPECT_GT(lethal_lower, 0u) << "the box is an obstacle on the robot's floor"; + EXPECT_EQ(lethal_upper, 0u) << "and not on the floor over it"; + EXPECT_GT(traversed_lower, 0u); + EXPECT_EQ(traversed_upper, 0u) << "the floor above was not driven over"; + // the robot goes up to the other floor (same x, y): a re-mesh in this very cycle + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + o.pose.pose.position.z = 3.0; + o.pose.pose.orientation.w = 1.0; + nav_state_.set("robot_pose", o); + see(false); + p->update(nav_state_); + const auto & after = nav_state_.get<::navmap::NavMap>("map.navmap"); + std::size_t traversed_upper_after = 0; + for (::navmap::NavCelId c = 0; c < after.navcels.size(); ++c) { + if (after.navcel_centroid(c).z() > 1.5f && + after.layer_get("traversed", c, 0) == 255) + { + ++traversed_upper_after; + } + } + EXPECT_GT(traversed_upper_after, 0u) << "ground driven over on the upper floor"; +} + +// Obstacle NavCels are painted in the published map: each gets its own three vertices with the +// tint, so a map with lethal NavCels (here: slopes steeper than max_slope_deg) has more vertices +// in the message than the mesh, and vertex colours even without imagery +TEST_F(BigMapPluginTest, ThePublishedMapPaintsTheObstacleNavCelsWithTheirOwnVertices) +{ + // a DEM with a cliff: the grid engine marks its NavCels LETHAL in `occupancy` + bigmap_maps_manager::DemGrid g; + g.side = 65; + g.resolution = 0.5; + g.data_resolution = 0.5; + g.radius = 16.0; + g.z.resize(65 * 65); + for (int j = 0; j < 65; ++j) { + for (int i = 0; i < 65; ++i) { + g.z[static_cast(j) * 65 + i] = i > 40 ? 4.0f : 0.0f; // a 4 m step + } + } + const auto dem = (std::filesystem::temp_directory_path() / + ("bigmap_plugin_test_cliff_" + std::to_string(::getpid()) + ".bin")).string(); + ASSERT_TRUE(g.save(dem)); + overrides_.erase( + std::remove_if( + overrides_.begin(), overrides_.end(), + [](const rclcpp::Parameter & q) {return q.get_name() == "bigmap.terrain.initial_navmap";}), + overrides_.end()); + set("terrain.dem_file", dem); + set("obstacles.tint.enabled", true); + auto p = make(); + auto listener = std::make_shared("bigmap_plugin_test_tint_listener"); + std::vector maps; + auto sub = listener->create_subscription( + "/" + std::string(node_->get_name()) + "/bigmap/map", rclcpp::QoS(10).reliable(), + [&maps](navmap_ros_interfaces::msg::NavMap::ConstSharedPtr m) {maps.push_back(*m);}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(listener); + robot_at_origin(); + see(false); + p->update(nav_state_); + std::filesystem::remove(dem); + for (int k = 0; k < 20 && maps.empty(); ++k) { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ASSERT_FALSE(maps.empty()); + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + std::size_t lethal = 0; + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + lethal += nm.layer_get("occupancy", c, 0) == ::navmap::LETHAL_OBSTACLE; + } + ASSERT_GT(lethal, 0u) << "the step is too steep to drive"; + const auto & m = maps.back(); + EXPECT_TRUE(m.has_vertex_rgba); + EXPECT_EQ(m.positions_x.size(), nm.positions.x.size() + 3 * lethal) + << "every painted NavCel has its own three vertices"; + EXPECT_EQ(m.colors_r.size(), m.positions_x.size()); + EXPECT_EQ(m.navcels_v0.size(), nm.navcels.size()); + // the painted vertices are the tint (red 255, 40, 40 at 0.65 over grey 170) + std::size_t red = 0; + for (std::size_t v = nm.positions.x.size(); v < m.positions_x.size(); ++v) { + red += m.colors_r[v] > m.colors_g[v] + 50; + } + EXPECT_EQ(red, 3 * lethal); +} + +// ---- Audit benchmarks (not run by default): colcon test never pays for them. Run with +// test_bigmap_plugin --gtest_also_run_disabled_tests --gtest_filter='*Benchmark*' +namespace +{ +double rss_mb() +{ + std::ifstream f("/proc/self/statm"); + int64_t pages = 0, resident = 0; + f >> pages >> resident; + return static_cast(resident) * static_cast(sysconf(_SC_PAGESIZE)) / 1048576.0; +} + +double percentile(std::vector v, double p) +{ + std::sort(v.begin(), v.end()); + return v[static_cast(p * static_cast(v.size() - 1))]; +} +} // namespace + +// obstacles.detector navmap_filter: the ObstaclesFilter plugin of easynav_navmap_maps_manager +// detects (through the TF map -> robot), and bigmap removes what it marked and inflates around +TEST_F(BigMapPluginTest, TheFilterDetectorRemovesWhatTheObstacleFilterMarksAndReleasesItAfter) +{ + set("obstacles.detector", std::string("navmap_filter")); + set("filters", std::vector{"obstacle_filter", "inflation"}); + set("obstacle_filter.plugin", + std::string("easynav_navmap_maps_manager/NavMapMapsManager/ObstaclesFilter")); + set("lengths.radii.obstacle_clearance", 0.5); + // the filter measures through TF: the robot stands at the origin, its footprint under it + const auto info = easynav::RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::TransformStamped tf; + tf.header.frame_id = info.map_frame; + tf.child_frame_id = info.robot_frame; + tf.transform.rotation.w = 1.0; + easynav::RTTFBuffer::getInstance()->setTransform(tf, "test", true); + tf.header.frame_id = info.robot_frame; + tf.child_frame_id = info.robot_footprint_frame; + easynav::RTTFBuffer::getInstance()->setTransform(tf, "test", true); + auto p = make(); + robot_at_origin(); + run(*p, true, 0.4); + ASSERT_TRUE(nav_state_.has("map.navmap")); + EXPECT_GT(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u) + << "the filter saw the box and bigmap kept it LETHAL for the planner"; + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + ASSERT_TRUE(nm.layers.get("inflated_obstacles")); + std::size_t inflated_around = 0; + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto q = nm.navcel_centroid(c); + const double d = std::hypot(q.x() - 3.0, q.y()); + if (d > 0.6 && d < 1.2 && nm.layer_get("inflated_obstacles", c, 0) > 0) { + ++inflated_around; + } + } + EXPECT_GT(inflated_around, 0u) << "the inflation is built on this cycle's obstacles"; + // the published map has the detections removed (holes), with the clearance ring around them + // nothing in sight: the filter has no memory, the detections and our ring go away + run(*p, false, 0.4); + EXPECT_EQ(count_at_least(at_box("obstacles"), ::navmap::LETHAL_OBSTACLE), 0u); + easynav::RTTFBuffer::getInstance()->clear(); +} + +// EasyNav serves the services from its non-real-time executor while update() runs in the +// real-time one: the queue between them (mutex, atomic count of traversed nodes) must hold +// while both hammer it. Hundreds of requests from another thread against a robot that drives. +TEST_F(BigMapPluginTest, TheSaveServicesAreSafeToCallFromAnotherThreadWhileTheRobotDrives) +{ + const auto dir = std::filesystem::temp_directory_path() / + ("bigmap_plugin_test_concurrent_" + std::to_string(::getpid())); + set("output.save_directory", dir.string()); + auto p = make(); + robot_at_origin(); + see(false); + p->update(nav_state_); + std::atomic stop{false}; + std::atomic answered{0}, accepted{0}, busy{0}, nothing_traversed{0}, other{0}; + const std::string base = "/" + std::string(node_->get_name()) + "/bigmap/"; + // the services' thread: EasyNav's non-real-time executor + std::thread services([&]() { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); + while (!stop) {executor.spin_some(std::chrono::milliseconds(2));} + }); + // a client that asks for a save as often as it can, for traversed ground and for all + std::thread client([&]() { + auto client_node = std::make_shared("bigmap_plugin_test_hammer"); + std::vector::SharedPtr> clients = { + client_node->create_client(base + "save_traversed"), + client_node->create_client(base + "save_map")}; + for (auto & c : clients) { + c->wait_for_service(std::chrono::seconds(5)); + } + int k = 0; + while (!stop) { + auto & c = clients[k++ % 2]; + auto future = c->async_send_request(std::make_shared()); + if (rclcpp::spin_until_future_complete(client_node, future, std::chrono::seconds(2)) != + rclcpp::FutureReturnCode::SUCCESS) {continue;} + const auto res = future.get(); + ++answered; + if (res->success) { + ++accepted; + } else if (res->message.find("in progress") != std::string::npos) { + ++busy; + } else if (res->message.find("not driven") != std::string::npos) { + ++nothing_traversed; + } else { + ++other; + } + } + }); + // the robot drives (the real-time thread): marks ground, re-meshes, writes the saves. It drives + // until both sides have done enough, not for a fixed time: a coverage build on a shared CI + // runner takes ~250 ms per update() with saves + const auto end = std::chrono::steady_clock::now() + std::chrono::seconds(60); + int updates = 0; + for (double t = 0.0; + (updates <= 50 || answered.load() <= 20 || accepted.load() == 0) && + std::chrono::steady_clock::now() < end; + t += 0.1, ++updates) + { + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + o.pose.pose.position.x = 8.0 * std::sin(t); + o.pose.pose.position.y = 8.0 * std::cos(t); + o.pose.pose.orientation.w = 1.0; + nav_state_.set("robot_pose", o); + see(false); + ASSERT_NO_THROW(p->update(nav_state_)); + } + stop = true; + client.join(); + services.join(); + EXPECT_GT(updates, 50); + EXPECT_GT(answered.load(), 20) << "the services did not answer while the robot drove"; + EXPECT_GT(accepted.load(), 0); + EXPECT_EQ(other.load(), 0) << "an answer that is neither accepted, busy nor 'not driven'"; + // every accepted request was written (or refused by update() with a log, never lost) + std::size_t files = 0; + for (const auto & e : std::filesystem::directory_iterator(dir)) { + files += e.path().extension() == ".navmap"; + } + EXPECT_GT(files, 0u); + EXPECT_LE(files, static_cast(accepted.load())); + std::filesystem::remove_all(dir); +} + +TEST_F(BigMapPluginTest, DISABLED_BenchmarkUpdateAndMemoryWhileTheRobotDrives) +{ + std::filesystem::remove(map_file_); + const double half = + std::getenv("BIGMAP_BENCH_HALF") ? std::atof(std::getenv("BIGMAP_BENCH_HALF")) : + 50.0; + map_file_ = flat_navmap(half, 0.5); + overrides_.clear(); + set("terrain.initial_navmap", map_file_); + set("lengths.cell_size.min", 0.5); + set("lengths.cell_size.max", 8.0); + set("lengths.radii.full_res", 6.0); + set("lengths.radii.low_res_start", 12.0); + set("lengths.radii.traversed_keep", 12.0); + set("lengths.remesh_step", 0.5); + set("dynamic_map.ground_fit.enabled", false); + set("output.publish_markers", false); + set("output.profile.enabled", std::getenv("BIGMAP_BENCH_PROFILE") != nullptr); + set("output.profile.period", 3.0); + set("filters", std::vector{"inflation"}); + set("inflation.plugin", + std::string("easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter")); + set("inflation.inflation_radius", 1.0); + set("inflation.cost_scaling_factor", 3.0); + auto p = make(); + robot_at_origin(); + const int cycles = + std::getenv("BIGMAP_BENCH_CYCLES") ? std::atoi(std::getenv("BIGMAP_BENCH_CYCLES")) : + 600; + std::vector ms; + std::cerr << "cells " << nav_state_.has("map.navmap") << " cycle rss_MB\n"; + for (int k = 0; k < cycles; ++k) { + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + // a lap around a square of side half: new ground all the time, then the same ground again + const double s = std::fmod(0.4 * k, 8.0 * (half - 10.0)); + const double side = 2.0 * (half - 10.0); + const int leg = static_cast(s / side); + const double t = s - leg * side; + const double px[4] = {-(half - 10.0) + t, half - 10.0, (half - 10.0) - t, -(half - 10.0)}; + const double py[4] = {-(half - 10.0), -(half - 10.0) + t, half - 10.0, (half - 10.0) - t}; + o.pose.pose.position.x = px[leg % 4]; + o.pose.pose.position.y = py[leg % 4]; + o.pose.pose.orientation.w = 1.0; + nav_state_.set("robot_pose", o); + see(k % 3 == 0); + const auto t0 = std::chrono::steady_clock::now(); + p->update(nav_state_); + ms.push_back( + std::chrono::duration(std::chrono::steady_clock::now() - t0).count()); + if (k % 100 == 0 || k == cycles - 1) { + std::cerr << "cycle " << k << " rss_MB " << rss_mb() << "\n"; + } + } + if (nav_state_.has("map.navmap")) { + std::cerr << "navcels in the NavState map: " + << nav_state_.get<::navmap::NavMap>("map.navmap").navcels.size() << "\n"; + } + std::cerr << "update() ms: median " << percentile(ms, 0.5) << " p95 " << percentile(ms, 0.95) + << " p99 " << percentile(ms, 0.99) << " max " << percentile(ms, 1.0) << "\n"; +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_dem_grid.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_dem_grid.cpp new file mode 100644 index 00000000..90b76e09 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_dem_grid.cpp @@ -0,0 +1,268 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// The DEM file is also written by navmap_tools' navmap_gis_tool (Python, --dem-out). Both sides +// keep the same golden file (test/data/dem_golden.bin, made once by the C++ DemGrid::build and +// save of the former bigmap_dem package from a smooth function: 9 x 9 nodes of 1 m around +// 40.4, -3.7, sampled every 2 m and bicubically interpolated). This side checks that it loads and +// saves back byte by byte; navmap_tools' test checks that its writer reproduces the bytes and +// that its sampling agrees with the values. + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/dem_grid.hpp" + +using bigmap_maps_manager::DemGrid; + +namespace +{ +std::string golden() +{ + return (std::filesystem::path(BIGMAP_TEST_DATA) / "dem_golden.bin").string(); +} + +std::string bytes(const std::string & path) +{ + std::ifstream f(path, std::ios::binary); + return std::string(std::istreambuf_iterator(f), std::istreambuf_iterator()); +} +} // namespace + +TEST(DemGrid, GoldenFileLoadsWithItsHeaderAndHeights) +{ + const auto g = DemGrid::load(golden()); + ASSERT_TRUE(g.has_value()); + EXPECT_EQ(g->side, 9); + EXPECT_EQ(g->source, DemGrid::kCopernicus); + EXPECT_DOUBLE_EQ(g->resolution, 1.0); + EXPECT_DOUBLE_EQ(g->data_resolution, 2.0); + EXPECT_DOUBLE_EQ(g->radius, 4.0); + EXPECT_DOUBLE_EQ(g->origin_lat, 40.4); + EXPECT_DOUBLE_EQ(g->origin_lon, -3.7); + EXPECT_TRUE(std::isnan(g->requested_reference)); + EXPECT_NEAR(g->reference_altitude, 694.392, 1e-3); + ASSERT_EQ(g->z.size(), 81u); + EXPECT_TRUE(g->rgb.empty()); + EXPECT_NEAR(g->at(4, 4), 0.0, 1e-6) << "the origin is the reference"; + EXPECT_NEAR(g->at(0, 0), -25.29486, 1e-4); + EXPECT_NEAR(g->at(8, 8), 3.52368, 1e-4); + EXPECT_NEAR(g->at(6, 2), -2.40851, 1e-4); // i = east, j = north + EXPECT_NEAR(g->at(0, 8), -11.6571, 1e-3); +} + +TEST(DemGrid, SavedAgainItIsTheSameFileByteForByte) +{ + const auto g = DemGrid::load(golden()); + ASSERT_TRUE(g.has_value()); + const auto out = (std::filesystem::temp_directory_path() / "bigmap_dem_grid_again.bin").string(); + ASSERT_TRUE(g->save(out)); + EXPECT_EQ(bytes(out), bytes(golden())); + std::filesystem::remove(out); +} + +TEST(DemGrid, ColoursSurviveASaveAndLoad) +{ + auto g = *DemGrid::load(golden()); + g.imagery_zoom = 18; + g.rgb.assign(81 * 3, 7); + const auto out = (std::filesystem::temp_directory_path() / "bigmap_dem_grid_rgb.bin").string(); + ASSERT_TRUE(g.save(out)); + const auto back = DemGrid::load(out); + ASSERT_TRUE(back.has_value()); + EXPECT_EQ(back->imagery_zoom, 18); + EXPECT_EQ(back->rgb.size(), 81u * 3u); + std::filesystem::remove(out); +} + +TEST(DemGrid, TheCellsWithGroundSurviveASaveAndLoadAndAnOlderFileHasNone) +{ + auto g = *DemGrid::load(golden()); + EXPECT_TRUE(g.cell_valid.empty()) << "the golden file is older than the cells block"; + g.imagery_zoom = 18; + g.rgb.assign(81 * 3, 7); + g.cell_valid.assign(64, 1); + g.cell_valid[3 * 8 + 5] = 0; // a hole at cell (5, 3) + const auto out = (std::filesystem::temp_directory_path() / "bigmap_dem_grid_cells.bin").string(); + ASSERT_TRUE(g.save(out)); + const auto back = DemGrid::load(out); + ASSERT_TRUE(back.has_value()); + EXPECT_EQ(back->cell_valid, g.cell_valid); + EXPECT_EQ(back->rgb.size(), 81u * 3u); + // a cells block cut short or with another tag is a broken file, not a file without cells + const auto all = bytes(out); + for (const std::size_t cut : {std::size_t{1}, std::size_t{30}, std::size_t{64}}) { + std::ofstream(out, std::ios::binary | std::ios::trunc) << all.substr(0, all.size() - cut); + EXPECT_FALSE(DemGrid::load(out).has_value()) << "cut " << cut; + } + std::string other = all; + other[other.size() - 64 - 8] = 'X'; + std::ofstream(out, std::ios::binary | std::ios::trunc) << other; + EXPECT_FALSE(DemGrid::load(out).has_value()); + // the wrong number of cells is not written + g.cell_valid.resize(10); + EXPECT_FALSE(g.save(out)); + std::filesystem::remove(out); +} + +TEST(DemGrid, SubsampleKeepsEveryHole) +{ + auto g = *DemGrid::load(golden()); + g.cell_valid.assign(64, 1); + g.cell_valid[5 * 8 + 2] = 0; // cell (2, 5): in the coarse cell (1, 2) + const auto s = g.subsample(2); + ASSERT_EQ(s.cell_valid.size(), 16u); + for (int j = 0; j < 4; ++j) { + for (int i = 0; i < 4; ++i) { + EXPECT_EQ(s.cell_valid[j * 4 + i], (i == 1 && j == 2) ? 0 : 1) << i << ", " << j; + } + } +} + +TEST(DemGrid, HeightAtIsBilinearAndClampedToTheGrid) +{ + const auto g = *DemGrid::load(golden()); + EXPECT_NEAR(g.height_at(0.0, 0.0), 0.0, 1e-6); + EXPECT_NEAR(g.height_at(g.x(6), g.y(2)), g.at(6, 2), 1e-5); + EXPECT_NEAR(g.height_at(0.5, 0.0), 0.5 * (g.at(4, 4) + g.at(5, 4)), 1e-5); + EXPECT_NEAR(g.height_at(100.0, 100.0), g.at(8, 8), 1e-6); +} + +TEST(DemGrid, ANonFiniteCoordinateHasNoHeightInsteadOfReadingOutsideTheGrid) +{ + // a NaN went through std::clamp and the int cast (undefined: INT_MIN on x86) into z[] + const auto g = *DemGrid::load(golden()); + const double nan = std::numeric_limits::quiet_NaN(); + const double inf = std::numeric_limits::infinity(); + EXPECT_TRUE(std::isnan(g.height_at(nan, 0.0))); + EXPECT_TRUE(std::isnan(g.height_at(0.0, nan))); + EXPECT_TRUE(std::isnan(g.height_at(inf, 0.0))); + EXPECT_TRUE(std::isnan(g.height_at(0.0, -inf))); + EXPECT_NEAR(g.height_at(1e300, -1e300), g.at(8, 0), 1e-6); // huge but finite: clamped +} + +TEST(DemGrid, SubsampleKeepsEveryNthNodeAndRefusesWhatDoesNotDivide) +{ + const auto g = *DemGrid::load(golden()); + const auto s = g.subsample(2); + EXPECT_EQ(s.side, 5); + EXPECT_DOUBLE_EQ(s.resolution, 2.0); + EXPECT_FLOAT_EQ(s.at(1, 3), g.at(2, 6)); + EXPECT_THROW(g.subsample(3), std::invalid_argument); +} + +TEST(DemGrid, MissingOrBrokenFilesAreNotLoaded) +{ + EXPECT_FALSE(DemGrid::load("/nonexistent/dem.bin").has_value()); + const auto out = (std::filesystem::temp_directory_path() / "bigmap_dem_grid_broken.bin").string(); + { + std::ofstream f(out, std::ios::binary); + f << "LMDEM006 not really"; + } + EXPECT_FALSE(DemGrid::load(out).has_value()); + std::filesystem::remove(out); +} + +namespace +{ +// A DEM file written by hand: `side` and `resolution` as given, the heights and (optionally) the +// colours cut at `payload` floats / bytes, to tell what the loader trusts from what it checks +std::string craft( + std::int32_t side, double resolution, std::size_t floats, std::int32_t zoom = 0, + std::size_t rgb_bytes = 0) +{ + std::string out("LMDEM006", 8); + const std::int32_t source = 0; + const double hdr[7] = {resolution, 4.0, 40.4, -3.7, 600.0, 600.0, 2.0}; + out.append(reinterpret_cast(&side), sizeof(side)); + out.append(reinterpret_cast(&source), sizeof(source)); + out.append(reinterpret_cast(hdr), sizeof(hdr)); + out.append(floats * sizeof(float), '\0'); + out.append(reinterpret_cast(&zoom), sizeof(zoom)); + out.append(rgb_bytes, '\0'); + return out; +} + +std::optional load_bytes(const std::string & content) +{ + const auto path = (std::filesystem::temp_directory_path() / + ("bigmap_dem_grid_crafted_" + std::to_string(::getpid()) + ".bin")).string(); + { + std::ofstream f(path, std::ios::binary); + f.write(content.data(), static_cast(content.size())); + } + auto g = DemGrid::load(path); + std::filesystem::remove(path); + return g; +} +} // namespace + +TEST(DemGrid, AHeaderThatPromisesMoreHeightsThanTheFileHasIsRefusedBeforeAllocating) +{ + // 19999 x 19999 floats = 1.6 GB claimed by a 60-byte file + EXPECT_FALSE(load_bytes(craft(19999, 1.0, 10)).has_value()); +} + +TEST(DemGrid, ASideOfOneIsNotAGrid) +{ + // height_at() reads node (side - 2) = -1 of it + EXPECT_FALSE(load_bytes(craft(1, 1.0, 1)).has_value()); +} + +TEST(DemGrid, ANonPositiveOrNonFiniteResolutionIsRefused) +{ + for (const double r : {0.0, -1.0, std::numeric_limits::quiet_NaN(), + std::numeric_limits::infinity()}) + { + EXPECT_FALSE(load_bytes(craft(9, r, 81)).has_value()) << "resolution " << r; + } + EXPECT_TRUE(load_bytes(craft(9, 1.0, 81)).has_value()); +} + +TEST(DemGrid, AnImageryZoomWithoutAllItsColoursOrOutOfRangeIsRefused) +{ + EXPECT_FALSE(load_bytes(craft(9, 1.0, 81, 18, 10)).has_value()); // colours cut short + EXPECT_TRUE(load_bytes(craft(9, 1.0, 81, 18, 81 * 3)).has_value()); + EXPECT_FALSE(load_bytes(craft(9, 1.0, 81, -5, 81 * 3)).has_value()); // negative zoom +} + +TEST(DemGrid, RandomBytesAfterAValidHeaderNeverCrashTheLoader) +{ + std::mt19937 rng(12345); // fixed seed: a failure is reproducible + for (int n = 0; n < 300; ++n) { + std::string content = craft(9, 1.0, 81, n % 2 ? 18 : 0, n % 2 ? 81 * 3 : 0); + const std::size_t flips = 1 + rng() % 8; + for (std::size_t k = 0; k < flips; ++k) { + content[rng() % content.size()] = static_cast(rng()); + } + if (n % 5 == 0) {content.resize(rng() % content.size());} + const auto g = load_bytes(content); + if (g) { // whatever loads must be a usable grid + EXPECT_EQ(g->z.size(), static_cast(g->side) * g->side); + EXPECT_GE(g->side, 3); + EXPECT_TRUE(g->resolution > 0.0 && std::isfinite(g->resolution)); + } + } +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_garage_lod.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_garage_lod.cpp new file mode 100644 index 00000000..232dab0c --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_garage_lod.cpp @@ -0,0 +1,92 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// The hierarchy engine on the real garage map (bigmap_bringup/maps/parking_garage.navmap, the one +// the garage scenario uses, with its settings): the meshes must not fold over the surface. The +// source has no triangle facing down; the coarse levels of the hierarchy still fold a few (a +// cut that mixes levels can), so this bounds them (39 of ~40 000 before the fold check of +// build_levels, 33 after) instead of asking for none. + +#include + +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/surface_lod.hpp" +#include "navmap_core/NavMap.hpp" +#include "navmap_ros/navmap_io.hpp" + +namespace +{ + +// Triangles that face down (clockwise seen from above) +std::size_t facing_down(const bigmap_maps_manager::LodMesh & m) +{ + std::size_t n = 0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const double cross = (m.x[b] - m.x[a]) * (m.y[c] - m.y[a]) - + (m.y[b] - m.y[a]) * (m.x[c] - m.x[a]); + n += cross < 0.0; + } + return n; +} + +} // namespace + +TEST(GarageLod, TheMeshesDoNotFoldOverTheSurface) +{ + const char * from_env = std::getenv("BIGMAP_GARAGE_NAVMAP"); + const std::filesystem::path path = from_env ? from_env : BIGMAP_GARAGE_NAVMAP; + if (!std::filesystem::exists(path)) {GTEST_SKIP() << path << " not found";} + ::navmap::NavMap nm; + std::error_code ec; + ASSERT_TRUE(navmap_ros::io::load_from_file(path.string(), nm, &ec)) << ec.message(); + + bigmap_maps_manager::SurfaceLodMesher::Config cfg; // params_parking_garage.yaml + cfg.min_size = 0.4; + cfg.max_size = 16.0; + cfg.keep_narrow_width = 3.0; + cfg.drop_unreachable_ground = true; + bigmap_maps_manager::SurfaceLodMesher mesher(nm, cfg); + bigmap_maps_manager::LodParams p; + p.high_res_radius = 5.0; + p.low_res_distance = 20.0; + p.min_size = cfg.min_size; + p.max_size = cfg.max_size; + + float xmin = 1e9f, xmax = -1e9f, ymin = 1e9f, ymax = -1e9f; + for (std::size_t v = 0; v < nm.positions.x.size(); ++v) { + xmin = std::min(xmin, nm.positions.x[v]); xmax = std::max(xmax, nm.positions.x[v]); + ymin = std::min(ymin, nm.positions.y[v]); ymax = std::max(ymax, nm.positions.y[v]); + } + std::size_t total_down = 0, total = 0; + for (const float fx : {0.1f, 0.5f, 0.9f}) { + for (const float fy : {0.2f, 0.8f}) { + const auto m = mesher.extract( + xmin + fx * (xmax - xmin), ymin + fy * (ymax - ymin), 0.0, p); + ASSERT_GT(m.triangle_count(), 0u); + const auto down = facing_down(m); + std::printf("[garage] robot (%.1f, %.1f): %zu triangles, %zu facing down\n", + xmin + fx * (xmax - xmin), ymin + fy * (ymax - ymin), m.triangle_count(), down); + total_down += down; + total += m.triangle_count(); + } + } + EXPECT_LE(total_down * 500, total) << "more than 0.2 % of the triangles face down"; +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_lod_closure.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_lod_closure.cpp new file mode 100644 index 00000000..8dcf0bf6 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_lod_closure.cpp @@ -0,0 +1,372 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// The argument for a mesh without T-junctions (paper, Section 3.2), checked exhaustively on the +// whole implicit RTIN tree of random terrains, with random robots, driven-over ground, observed +// borders, holes and pinned (narrow passage) nodes: +// (a)+(b) the split decision is closed upwards: split(child) => split(parent), for every pair +// (this needs the nested error and kReach >= (1 + sqrt 2) / 2); +// the stored error is nested: e(child) <= e(parent), pinned(child) => pinned(parent); +// (c) the meshes extract() returns have no T-junctions. +// And what the error is NOT: the vertical deviation of a coarse triangle can exceed the stored +// error, so the tolerance bounds the hierarchical surplus, not the deviation. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/known_map.hpp" +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" +#include "easynav_bigmap_maps_manager/lod_policy.hpp" + +using namespace bigmap_maps_manager; // NOLINT(build/namespaces) + +namespace +{ + +DemGrid random_dem(int side, double res, std::mt19937 & rng, int kind) +{ + DemGrid g; + g.side = side; + g.resolution = res; + g.radius = (side - 1) / 2 * res; + g.z.resize(static_cast(side) * side); + std::normal_distribution n(0.0, 1.0); + const double a1 = n(rng), a2 = n(rng); + const double p1 = 5 + 20 * std::abs(n(rng)), p2 = 2 + 5 * std::abs(n(rng)); + for (int j = 0; j < side; ++j) { + for (int i = 0; i < side; ++i) { + const double x = g.x(i), y = g.y(j); + double z = 0.0; + if (kind == 0) { // smooth relief and noise + z = 3.0 * a1 * std::sin(x / p1) * std::cos(y / p1) + + 0.5 * a2 * std::sin(x / p2 + y / (p2 + 1)) + 0.05 * n(rng); + } else if (kind == 1) { // steps (raised blocks) + z = ((static_cast(std::floor(x / 7.0)) + static_cast(std::floor(y / 5.0))) % 3 == + 0) ? 1.5 : 0.0; + } else { // white noise + z = 0.3 * n(rng); + } + g.z[static_cast(j) * side + i] = static_cast(z); + } + } + return g; +} + +// The triangles of the implicit RTIN tree, with the vertices Extractor uses (c: right angle) +struct Tri +{ + int ax, ay, bx, by, cx, cy; + int parent; +}; + +void enumerate(std::vector & out, int ax, int ay, int bx, int by, int cx, int cy, int parent) +{ + const int me = static_cast(out.size()); + out.push_back({ax, ay, bx, by, cx, cy, parent}); + if (std::abs(ax - cx) + std::abs(ay - cy) <= 1) {return;} + const int mx = (ax + bx) >> 1, my = (ay + by) >> 1; + enumerate(out, cx, cy, ax, ay, mx, my, me); + enumerate(out, bx, by, cx, cy, mx, my, me); +} + +bool splittable(const Tri & t) {return std::abs(t.ax - t.cx) + std::abs(t.ay - t.cy) > 1;} + +// The decision of Extractor for one triangle (the same effective distances) +struct Decision +{ + const LodMesher & m; + const LodParams & p; + LodParams po; + double rx, ry; + const KnownMap * known; + const KnownMap * observed; + + bool operator()(const Tri & t) const + { + if (!splittable(t)) {return false;} + const DemGrid & dem = m.dem(); + const int mx = (t.ax + t.bx) >> 1, my = (t.ay + t.by) >> 1; + const double leg = std::hypot(t.ax - t.cx, t.ay - t.cy) * dem.resolution; + const double hyp = leg * M_SQRT2; + const int center = (dem.side - 1) / 2; + const double dist = std::hypot((mx - center) * dem.resolution - rx, + (my - center) * dem.resolution - ry); + double d = effective_distance(dist, hyp); + if (known && !known->empty()) { + d = std::min(d, known_effective_distance(p, known->distance(mx, my), hyp)); + } + if (observed && !observed->empty()) { + d = std::min(d, known_effective_distance(po, observed->distance(mx, my), hyp)); + } + const auto & em = m.error_map(); + return em.pinned(mx, my) || should_split_at(p, em.error(mx, my), leg, d); + } +}; + +// Edges used by one triangle only with a mesh vertex strictly inside: a T-junction +std::size_t t_junctions(const LodMesh & m, double res) +{ + using Edge = std::pair; + std::map use; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + for (int k = 0; k < 3; ++k) { + const uint32_t a = m.tri[3 * t + k], b = m.tri[3 * t + (k + 1) % 3]; + ++use[{std::min(a, b), std::max(a, b)}]; + } + } + std::set> nodes; // NOLINT(runtime/int) + auto node = [&](uint32_t v) { + return std::make_pair(std::lround(m.x[v] / res), std::lround(m.y[v] / res)); + }; + for (uint32_t v = 0; v < m.x.size(); ++v) { + nodes.insert(node(v)); + } + std::size_t bad = 0; + for (const auto & [e, n] : use) { + if (n != 1) {continue;} + const auto a = node(e.first), b = node(e.second); + const long dx = b.first - a.first, dy = b.second - a.second; // NOLINT(runtime/int) + const long g = std::gcd(std::labs(dx), std::labs(dy)); // NOLINT(runtime/int) + for (long k = 1; k < g; ++k) { // NOLINT(runtime/int) + if (nodes.count({a.first + dx / g * k, a.second + dy / g * k})) {++bad;} + } + } + return bad; +} + +// The warehouse configuration (Section 4.2), scaled to the resolution of the test DEM +LodParams warehouse_params(double res) +{ + LodParams p; + p.min_size = res; + p.max_size = 64 * res; + p.high_res_radius = 30 * res; + p.low_res_distance = 120 * res; + p.base_error = 0.02; + p.error_growth = 0.05; + return p; +} + +struct Totals +{ + long nest_pairs = 0, nest_bad = 0, pinned_bad = 0; // NOLINT(runtime/int) + long decision_pairs = 0, decision_bad = 0; // NOLINT(runtime/int) + long meshes = 0, meshes_with_t = 0; // NOLINT(runtime/int) + double worst_deviation_ratio = 0.0; // vertical deviation / stored error (error > 5 cm) +}; + +// 36 terrains (6 sizes, 2 resolutions, 3 kinds, with and without holes, hole_error and pinned +// nodes), 12 robots and memories each +const Totals & exhaustive() +{ + static Totals r; + static bool done = false; + if (done) {return r;} + done = true; + std::mt19937 rng(20261026); + for (int trial = 0; trial < 36; ++trial) { + const int sides[] = {33, 65, 129, 50, 100, 97}; + const int side = sides[trial % 6]; + const double res = (trial % 2) ? 0.5 : 1.0; + DemGrid dem = random_dem(side, res, rng, trial % 3); + std::vector cell_valid, pinned; + std::bernoulli_distribution hole(0.04), pin(0.003); + const bool with_holes = trial % 4 >= 2; + if (with_holes) { + cell_valid.assign(static_cast(side - 1) * (side - 1), 1); + for (auto & c : cell_valid) { + c = hole(rng) ? 0 : 1; + } + } + if (trial % 5 == 0) { + pinned.assign(static_cast(side) * side, 0); + for (auto & q : pinned) { + q = pin(rng) ? 1 : 0; + } + } + const double hole_error = with_holes && (trial % 3 == 0) ? 0.5 : 0.0; + LodMesher mesher(dem, {}, pinned, cell_valid, hole_error); + const auto & em = mesher.error_map(); + const int ts = em.tile_size(), last = side - 1; + std::vector T; + enumerate(T, 0, 0, ts, ts, ts, 0, -1); + enumerate(T, ts, ts, 0, 0, 0, ts, -1); + for (const auto & t : T) { // the stored error and the pinned flag are nested + if (t.parent < 0 || !splittable(t)) {continue;} + const Tri & P = T[t.parent]; + const int mx = (t.ax + t.bx) >> 1, my = (t.ay + t.by) >> 1; + const int px = (P.ax + P.bx) >> 1, py = (P.ay + P.by) >> 1; + ++r.nest_pairs; + r.nest_bad += em.error(mx, my) > em.error(px, py); + r.pinned_bad += em.pinned(mx, my) && !em.pinned(px, py); + } + for (const auto & t : T) { // vertical deviation of the triangle against its stored error + if (!splittable(t)) {continue;} + if (std::max({t.ax, t.bx, t.cx}) > last || std::max({t.ay, t.by, t.cy}) > last) {continue;} + const int mx = (t.ax + t.bx) >> 1, my = (t.ay + t.by) >> 1; + const double e = em.error(mx, my); + if (e <= 0.05) {continue;} + const double x0 = t.ax, y0 = t.ay, x1 = t.bx, y1 = t.by, x2 = t.cx, y2 = t.cy; + const double z0 = em.height(t.ax, t.ay), z1 = em.height(t.bx, t.by), + z2 = em.height(t.cx, t.cy); + const double det = (y1 - y2) * (x0 - x2) + (x2 - x1) * (y0 - y2); + double dev = 0.0; + for (int j = std::min({t.ay, t.by, t.cy}); j <= std::max({t.ay, t.by, t.cy}); ++j) { + for (int i = std::min({t.ax, t.bx, t.cx}); i <= std::max({t.ax, t.bx, t.cx}); ++i) { + const double l0 = ((y1 - y2) * (i - x2) + (x2 - x1) * (j - y2)) / det; + const double l1 = ((y2 - y0) * (i - x2) + (x0 - x2) * (j - y2)) / det; + const double l2 = 1.0 - l0 - l1; + if (l0 < -1e-9 || l1 < -1e-9 || l2 < -1e-9) {continue;} + dev = std::max(dev, std::abs(l0 * z0 + l1 * z1 + l2 * z2 - em.height(i, j))); + } + } + r.worst_deviation_ratio = std::max(r.worst_deviation_ratio, dev / e); + } + const double half = (side - 1) / 2 * res; + std::uniform_real_distribution U(-1.6 * half, 1.6 * half); + LodParams p = warehouse_params(res); + for (int q = 0; q < 12; ++q) { + p.known_margin = (q % 3) * 2.0 * res; + p.known_falloff = (q % 4 == 0) ? 1.0 : (q % 4 == 1 ? 3.0 : 40.0); + p.observed_margin = (q % 2) ? -1.0 : 1.0; + KnownMap known(dem.side, dem.resolution), observed(dem.side, dem.resolution); + const bool use_known = q % 3 != 0, use_observed = q % 4 >= 2; + if (use_known) { + const double x0 = U(rng), y0 = U(rng), x1 = U(rng), y1 = U(rng); + for (double s = 0; s <= 1.0; s += 0.01) { + known.mark(x0 + s * (x1 - x0), y0 + s * (y1 - y0), 1.0 * res); + } + known.update(); + } + if (use_observed) { + std::uniform_int_distribution I(0, side - 1); + for (int k = 0; k < 40; ++k) { + observed.mark_node(I(rng), I(rng)); + } + observed.update(); + } + const double rx = U(rng), ry = U(rng); + const Decision decide{mesher, p, observed_params(p), rx, ry, + use_known ? &known : nullptr, use_observed ? &observed : nullptr}; + std::vector D(T.size()); + for (std::size_t i = 0; i < T.size(); ++i) { + D[i] = decide(T[i]); + } + for (std::size_t i = 0; i < T.size(); ++i) { + if (T[i].parent < 0) {continue;} + ++r.decision_pairs; + r.decision_bad += D[i] && !D[T[i].parent]; + } + const auto m = mesher.extract(rx, ry, p, use_known ? &known : nullptr, + use_observed ? &observed : nullptr); + ++r.meshes; + r.meshes_with_t += t_junctions(m, res) > 0; + } + } + std::cout << "[closure] " << r.nest_pairs << " child-parent errors, " << r.decision_pairs << + " child-parent decisions, " << r.meshes << + " meshes; worst vertical deviation / stored error " << + r.worst_deviation_ratio << std::endl; + return r; +} + +} // namespace + +TEST(LodClosure, TheReachConstantNestsTheDiscsOfParentAndChild) +{ + // (a): split vertices h / (2 sqrt 2) apart, child hypotenuse h / sqrt 2. The worst child has its + // vertex that much closer to the robot; d_eff(parent) <= d_eff(child) iff + // k h >= h / (2 sqrt 2) + k h / sqrt 2, i.e. k >= (1 + sqrt 2) / 2 + EXPECT_GE(kReach, (1.0 + std::sqrt(2.0)) / 2.0); + for (double h : {1.0, 8.0, 45.0}) { + for (double d : {0.0, 3.0, 37.0, 500.0}) { + const double child = std::max(0.0, d - h / (2.0 * std::sqrt(2.0))); + EXPECT_LE(effective_distance(d, h), effective_distance(child, h / std::sqrt(2.0)) + 1e-12); + } + } +} + +TEST(LodClosure, TheStoredErrorAndThePinnedFlagAreNested) +{ + const auto & r = exhaustive(); + EXPECT_GT(r.nest_pairs, 100000); + EXPECT_EQ(r.nest_bad, 0); + EXPECT_EQ(r.pinned_bad, 0); +} + +TEST(LodClosure, SplittingACellAlwaysSplitsItsParent) +{ + const auto & r = exhaustive(); + EXPECT_GT(r.decision_pairs, 1000000); + EXPECT_EQ(r.decision_bad, 0); +} + +TEST(LodClosure, ExtractedMeshesHaveNoTJunctions) +{ + const auto & r = exhaustive(); + EXPECT_EQ(r.meshes, 432); + EXPECT_EQ(r.meshes_with_t, 0); +} + +// The tolerance bounds the hierarchical surplus, not the vertical deviation: on random terrain a +// coarse triangle deviates more than its stored error (the paper reports the worst ratio), and on +// a surface built with the same surplus E at every level the root triangle deviates several E. +TEST(LodClosure, TheStoredErrorIsTheSurplusNotTheDeviation) +{ + EXPECT_GT(exhaustive().worst_deviation_ratio, 1.0); + const int side = 129; + const double E = 0.25; + DemGrid g; + g.side = side; + g.resolution = 1.0; + g.radius = (side - 1) / 2; + g.z.assign(static_cast(side) * side, 0.0f); + std::vector set(g.z.size(), 0); + auto at = [&](int x, int y) -> float & {return g.z[static_cast(y) * side + x];}; + std::vector T; + enumerate(T, 0, 0, side - 1, side - 1, side - 1, 0, -1); + enumerate(T, side - 1, side - 1, 0, 0, 0, side - 1, -1); + std::stable_sort(T.begin(), T.end(), [](const Tri & a, const Tri & b) { + return std::abs(a.ax - a.cx) + std::abs(a.ay - a.cy) > + std::abs(b.ax - b.cx) + std::abs(b.ay - b.cy); + }); + for (const auto & t : T) { // coarse to fine: every midpoint E above its hypotenuse's mean + if (!splittable(t)) {continue;} + const int mx = (t.ax + t.bx) >> 1, my = (t.ay + t.by) >> 1; + const std::size_t k = static_cast(my) * side + mx; + if (set[k]) {continue;} + at(mx, my) = static_cast((at(t.ax, t.ay) + at(t.bx, t.by)) / 2.0 + E); + set[k] = 1; + } + LodMesher m(g); + double stored = 0.0, deviation = 0.0; + for (int j = 0; j < side; ++j) { + for (int i = 0; i < side; ++i) { + stored = std::max(stored, static_cast(m.error_map().error(i, j))); + if (i >= j) {deviation = std::max(deviation, static_cast(at(i, j)));} + } + } + EXPECT_NEAR(stored, E, 0.011); // the stored error is E (rounded up to 1 cm) + EXPECT_GT(deviation, 4.0 * E); // the root triangle (z = 0) deviates far more +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_lod_mesher.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_lod_mesher.cpp new file mode 100644 index 00000000..ad2f4f78 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_lod_mesher.cpp @@ -0,0 +1,1110 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/known_map.hpp" +#include "easynav_bigmap_maps_manager/profiler.hpp" +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" +#include "easynav_bigmap_maps_manager/narrow_passages.hpp" + +using bigmap_maps_manager::LodMesh; +using bigmap_maps_manager::LodMesher; +using bigmap_maps_manager::LodParams; + +namespace +{ +// 65x65 nodes (= 2^6 + 1, no padding), 2 m apart: rough terrain, so errors are non-trivial +bigmap_maps_manager::DemGrid make_dem() +{ + bigmap_maps_manager::DemGrid g; + g.side = 65; + g.resolution = 2.0; + g.radius = 64.0; + g.z.resize(65 * 65); + for (int j = 0; j < 65; ++j) { + for (int i = 0; i < 65; ++i) { + const double x = g.x(i), y = g.y(j); + g.z[j * 65 + i] = static_cast( + 8.0 * std::sin(x / 17.0) * std::cos(y / 13.0) + 0.6 * std::sin(x / 3.1 + y / 2.3)); + } + } + return g; +} + +LodParams params() +{ + LodParams p; + p.high_res_radius = 20.0; + p.min_size = 2.0; + p.max_size = 32.0; + p.base_error = 0.05; + p.error_growth = 0.1; + p.low_res_distance = 100.0; + return p; +} + +using Edge = std::pair; +std::map edge_use(const LodMesh & m) +{ + std::map use; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + for (int k = 0; k < 3; ++k) { + uint32_t a = m.tri[3 * t + k], b = m.tri[3 * t + (k + 1) % 3]; + ++use[{std::min(a, b), std::max(a, b)}]; + } + } + return use; +} +} // namespace + +TEST(LodMesher, CrackFreeAndManifoldForManyRobotPositions) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + const double half = 64.0; + for (double rx : {-60.0, -17.3, 0.0, 33.3, 64.0}) { + for (double ry : {-64.0, -5.0, 0.0, 41.0}) { + const auto m = mesher.extract(rx, ry, params()); + for (const auto & [e, n] : edge_use(m)) { + ASSERT_LE(n, 2) << "non-manifold edge"; + if (n == 1) { // a boundary edge must lie on the outer border: otherwise it is a crack + auto on_border = [&](uint32_t v) { + return std::abs(std::abs(m.x[v]) - half) < 1e-3 || + std::abs(std::abs(m.y[v]) - half) < 1e-3; + }; + ASSERT_TRUE(on_border(e.first) && on_border(e.second)) + << "crack at robot (" << rx << ", " << ry << ")"; + } + } + } + } +} + +TEST(LodMesher, CrackFreeUnderSteepPolicies) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + for (double growth : {0.5, 2.0, 10.0}) { + for (double radius : {0.0, 6.0, 25.0}) { + LodParams p = params(); + p.high_res_radius = radius; + p.error_growth = growth; + p.low_res_distance = radius + 4.0 / growth; // from very short to long transitions + p.base_error = 0.01; + p.max_size = 64.0; + for (double rx = -64.0; rx <= 64.0; rx += 16.0) { + const auto m = mesher.extract(rx, 7.0, p); + for (const auto & [e, n] : edge_use(m)) { + ASSERT_LE(n, 2); + if (n == 1) { + auto on_border = [&](uint32_t v) { + return std::abs(std::abs(m.x[v]) - 64.0) < 1e-3 || + std::abs(std::abs(m.y[v]) - 64.0) < 1e-3; + }; + ASSERT_TRUE(on_border(e.first) && on_border(e.second)) + << "crack: growth " << growth << " radius " << radius << " rx " << rx; + } + } + } + } + } +} + +TEST(Profiler, ReportsTheSectionsSortedByTheTimeTheyTakeAndStartsANewWindow) +{ + bigmap_maps_manager::Profiler prof(3600.0); + EXPECT_FALSE(prof.due()); + prof.add("cheap", 1.0); + prof.add("expensive", 30.0); + prof.add("expensive", 10.0); + {bigmap_maps_manager::ProfScope s(&prof, "scoped");} + {bigmap_maps_manager::ProfScope s(nullptr, "never");} // a null profiler does nothing + const std::string r = prof.report_and_reset("title"); + EXPECT_NE(r.find("title"), std::string::npos); + EXPECT_NE(r.find("scoped"), std::string::npos); + EXPECT_EQ(r.find("never"), std::string::npos); + EXPECT_LT(r.find("expensive"), r.find("cheap")); // sorted: the costly one first + // the mean of "expensive" is 20 ms over 2 calls, the worst 30 + EXPECT_NE(r.find(" 2 20.00 30.00"), std::string::npos) << r; + EXPECT_EQ(prof.report_and_reset("again").find("expensive"), std::string::npos); +} + +TEST(KnownMap, DistanceIsTheExactEuclideanDistanceToTheNearestMarkedNode) +{ + bigmap_maps_manager::KnownMap k(65, 2.0); + EXPECT_TRUE(k.empty()); + k.mark(-30.0, 10.0, 1.0); + k.mark(20.0, -40.0, 3.0); + k.update(); + EXPECT_FALSE(k.empty()); + for (int j = 0; j < 65; j += 4) { + for (int i = 0; i < 65; i += 3) { + double best = 1e9; + for (int q = 0; q < 65; ++q) { + for (int p = 0; p < 65; ++p) { + if (k.known(p, q)) {best = std::min(best, std::hypot(p - i, q - j) * 2.0);} + } + } + EXPECT_NEAR(k.distance(i, j), best, 1e-3) << i << "," << j; + } + } +} + +TEST(KnownMap, UpdateWithinIgnoresTheTraversedNodesBeyondTheRadiusButKeepsThemMarked) +{ + bigmap_maps_manager::KnownMap k(65, 2.0); + k.mark(-40.0, 0.0, 1.0); // far from (30, 0) + k.mark(20.0, 0.0, 1.0); // 10 m from it + k.update_within(30.0, 0.0, 15.0); + const int c = 32; + EXPECT_NEAR(k.distance(c + 10, c), 0.0, 1e-6); // x = 20: a source + EXPECT_GT(k.distance(c - 20, c), 50.0); // x = -40: not a source: far from 20 + EXPECT_TRUE(k.known(c - 20, c)); // but still marked + EXPECT_TRUE(k.near_marked(c - 20, c + 1, 1)); + EXPECT_FALSE(k.near_marked(c - 20, c + 3, 1)); + k.update_within(30.0, 0.0, 1.0); // nothing within 1 m + EXPECT_EQ(k.sources(), 0u); + EXPECT_TRUE(std::isinf(k.distance(c, c))); + k.update(); // all of them again + EXPECT_EQ(k.sources(), k.count()); + EXPECT_NEAR(k.distance(c - 20, c), 0.0, 1e-6); +} + +TEST(KnownMap, ForgetOutsideUnmarksForGoodUntilTheRobotDrivesOverItAgain) +{ + bigmap_maps_manager::KnownMap k(65, 2.0); + k.mark(-40.0, 0.0, 1.0); + k.mark(20.0, 0.0, 1.0); + const std::size_t before = k.count(); + const int c = 32; + EXPECT_EQ(k.forget_outside(30.0, 0.0, 15.0), before / 2); // the one at -40 m (70 m away) + k.update(); + EXPECT_FALSE(k.known(c - 20, c)); + EXPECT_TRUE(k.known(c + 10, c)); + EXPECT_GT(k.distance(c - 20, c), 50.0); // no longer fine there + // the robot is back near x = -40: nothing returns by itself ... + k.forget_outside(-40.0, 0.0, 15.0); // the other one is the far one now + k.update(); + EXPECT_FALSE(k.known(c - 20, c)); + EXPECT_EQ(k.count(), 0u); + // ... until it drives over it again + k.mark(-40.0, 0.0, 1.0); + k.update(); + EXPECT_TRUE(k.known(c - 20, c)); + EXPECT_NEAR(k.distance(c - 20, c), 0.0, 1e-6); +} + +TEST(LodMesher, GroundTheRobotHasDrivenOverStaysFineWhereverTheRobotIsAndStaysCrackFree) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + bigmap_maps_manager::KnownMap known(dem.side, dem.resolution); + // a trail along y = -40, x from -60 to -20 + for (double x = -60.0; x <= -20.0; x += 1.0) { + known.mark(x, -40.0, 1.0); + } + known.update(); + LodParams p = params(); + p.known_margin = 2.0; + + const double rx = 50.0, ry = 50.0; // the robot is far away + const auto without = mesher.extract(rx, ry, p); + const auto with = mesher.extract(rx, ry, p, &known); + auto leaf_at = [](const LodMesh & m, double x, double y) { + double best = 1e9, leg = -1.0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + double cx = 0, cy = 0; + for (int k = 0; k < 3; ++k) { + cx += m.x[m.tri[3 * t + k]] / 3; + cy += m.y[m.tri[3 * t + k]] / 3; + } + const double d = std::hypot(cx - x, cy - y); + if (d < best) {best = d; leg = m.leg[t];} + } + return leg; + }; + EXPECT_GT(leaf_at(without, -40.0, -40.0), p.min_size) << "the point is far: it should be coarse"; + EXPECT_DOUBLE_EQ(leaf_at(with, -40.0, -40.0), p.min_size); + EXPECT_GT(with.triangle_count(), without.triangle_count()); + + // no cracks: an edge used once must lie on the border of the domain + for (const auto & [e, n] : edge_use(with)) { + ASSERT_LE(n, 2); + if (n == 1) { + auto on_border = [&](uint32_t v) { + return std::abs(std::abs(with.x[v]) - 64.0) < 1e-3 || + std::abs(std::abs(with.y[v]) - 64.0) < 1e-3; + }; + ASSERT_TRUE(on_border(e.first) && on_border(e.second)) << "T-junction with a trail"; + } + } +} + +TEST(LodMesher, TraversedGroundFadesWithinTheShortFalloffAndOnlyThere) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + bigmap_maps_manager::KnownMap known(dem.side, dem.resolution); + for (double x = -60.0; x <= -20.0; x += 1.0) { + known.mark(x, -40.0, 1.0); + } + known.update(); + LodParams p = params(); + p.known_margin = 6.0; + p.known_falloff = (p.low_res_distance - p.high_res_radius) / 4.0; // fade over 4 m + + const double rx = 50.0, ry = 50.0; + const auto with = mesher.extract(rx, ry, p, &known); + const auto without = mesher.extract(rx, ry, p); + auto leaf_at = [](const LodMesh & m, double x, double y) { + double best = 1e9, leg = -1.0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + double cx = 0, cy = 0; + for (int k = 0; k < 3; ++k) { + cx += m.x[m.tri[3 * t + k]] / 3; + cy += m.y[m.tri[3 * t + k]] / 3; + } + const double d = std::hypot(cx - x, cy - y); + if (d < best) {best = d; leg = m.leg[t];} + } + return leg; + }; + EXPECT_DOUBLE_EQ(leaf_at(with, -40.0, -40.0), p.min_size); + EXPECT_DOUBLE_EQ(leaf_at(with, -40.0, -36.0), p.min_size); // inside the radius (6 m) + // beyond the radius the fade is much faster than the robot's own (known_falloff = 1): coarser + // there and far fewer triangles in all (a crack-free mesh can only grow about one leg per + // 1.8 legs of distance, so it cannot be a step) + LodParams slow = p; + slow.known_falloff = 1.0; + const auto slow_mesh = mesher.extract(rx, ry, slow, &known); + EXPECT_GT(leaf_at(with, -40.0, -40.0 + 6.0 + 8.0), leaf_at(slow_mesh, -40.0, -26.0)); + EXPECT_LT(with.triangle_count(), slow_mesh.triangle_count()); + EXPECT_GT(with.triangle_count(), without.triangle_count()); + for (const auto & [e, n] : edge_use(with)) { + ASSERT_LE(n, 2); + if (n == 1) { + auto on_border = [&](uint32_t v) { + return std::abs(std::abs(with.x[v]) - 64.0) < 1e-3 || + std::abs(std::abs(with.y[v]) - 64.0) < 1e-3; + }; + ASSERT_TRUE(on_border(e.first) && on_border(e.second)) << "T-junction"; + } + } +} + +TEST(LodMesher, TheTwoCirclesKeepTheTraversedGroundFineOnlyInsideTheLargeOne) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + bigmap_maps_manager::KnownMap known(dem.side, dem.resolution); + for (double x = -60.0; x <= 40.0; x += 1.0) { + known.mark(x, -30.0, 1.0); + } // a long trail + LodParams p = params(); + p.high_res_radius = 10.0; + p.low_res_distance = 30.0; + p.known_margin = 2.0; + p.known_falloff = (p.low_res_distance - p.high_res_radius) / 3.0; + auto leaf_at = [](const LodMesh & m, double x, double y) { + double best = 1e9, leg = -1.0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + double cx = 0, cy = 0; + for (int k = 0; k < 3; ++k) { + cx += m.x[m.tri[3 * t + k]] / 3; + cy += m.y[m.tri[3 * t + k]] / 3; + } + const double d = std::hypot(cx - x, cy - y); + if (d < best) {best = d; leg = m.leg[t];} + } + return leg; + }; + const double rx = 0.0, ry = 30.0; // the robot is 60 m from the trail's centre + + // mode persistent: the whole trail is fine + known.update(); + const auto persistent = mesher.extract(rx, ry, p, &known); + EXPECT_DOUBLE_EQ(leaf_at(persistent, -50.0, -30.0), p.min_size); + EXPECT_DOUBLE_EQ(leaf_at(persistent, 0.0, -30.0), p.min_size); + + // mode circles, large circle 70 m around the robot: the trail within it is fine, the rest is + // coarse + known.update_within(rx, ry, 70.0); + const auto circles = mesher.extract(rx, ry, p, &known); + EXPECT_DOUBLE_EQ(leaf_at(circles, 0.0, -30.0), p.min_size); // 60 m away: inside 70 + EXPECT_GT(leaf_at(circles, -50.0, -30.0), p.min_size); // 78 m away: forgotten + + known.update_within(rx, ry, 50.0); // the large circle is smaller + const auto tight = mesher.extract(rx, ry, p, &known); + EXPECT_GT(leaf_at(tight, 0.0, -30.0), p.min_size); // 60 m away: outside 50 + EXPECT_LT(tight.triangle_count(), circles.triangle_count()); + EXPECT_LT(circles.triangle_count(), persistent.triangle_count()); + + // none of them has T-junctions + for (const auto * m : {&persistent, &circles, &tight}) { + for (const auto & [e, n] : edge_use(*m)) { + ASSERT_LE(n, 2); + if (n == 1) { + auto on_border = [&](uint32_t v) { + return std::abs(std::abs(m->x[v]) - 64.0) < 1e-3 || + std::abs(std::abs(m->y[v]) - 64.0) < 1e-3; + }; + ASSERT_TRUE(on_border(e.first) && on_border(e.second)) << "T-junction"; + } + } + } +} + +TEST(LodPolicy, SizeCapIsNearFineFarCoarseAndGeometricBetween) +{ + LodParams p; + p.high_res_radius = 10.0; + p.low_res_distance = 200.0; + p.min_size = 1.0; + p.max_size = 32.0; + using bigmap_maps_manager::size_cap; + EXPECT_DOUBLE_EQ(size_cap(p, 0.0), 1.0); + EXPECT_DOUBLE_EQ(size_cap(p, 10.0), 1.0); + EXPECT_DOUBLE_EQ(size_cap(p, 200.0), 32.0); + EXPECT_DOUBLE_EQ(size_cap(p, 1000.0), 32.0); + // halfway in distance -> geometric mean of the sizes: sqrt(1 * 32) + EXPECT_NEAR(size_cap(p, 105.0), std::sqrt(32.0), 1e-9); + double prev = 0.0; + for (double d = 0.0; d <= 300.0; d += 1.0) { // never decreases with distance + const double c = size_cap(p, d); + ASSERT_GE(c, prev); + prev = c; + } +} + +TEST(LodMesher, TriangulationCoversTheWholeDomainExactlyOnce) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + const auto m = mesher.extract(10.0, -20.0, params()); + double area = 0.0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const double cross = (m.x[b] - m.x[a]) * (m.y[c] - m.y[a]) - + (m.y[b] - m.y[a]) * (m.x[c] - m.x[a]); + ASSERT_GT(cross, 0.0) << "triangle not CCW"; + area += 0.5 * cross; + } + EXPECT_NEAR(area, 128.0 * 128.0, 1e-3); +} + +TEST(LodMesher, FineNearRobotCoarseFarAndFewerTriangles) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + const auto p = params(); + const auto m = mesher.extract(-40.0, -40.0, p); + + const std::size_t full = 2 * 64 * 64; + EXPECT_LT(m.triangle_count(), full / 3); + + float far_leg = 0.0f; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const double cx = (m.x[a] + m.x[b] + m.x[c]) / 3.0, cy = (m.y[a] + m.y[b] + m.y[c]) / 3.0; + const double d = std::hypot(cx + 40.0, cy + 40.0); + if (d < p.high_res_radius * 0.5) { + ASSERT_LE(m.leg[t], dem.resolution + 1e-4) << "not full resolution near the robot"; + } + far_leg = std::max(far_leg, m.leg[t]); + } + EXPECT_GT(far_leg, 4.0f * dem.resolution); +} + +TEST(LodMesher, MeshFollowsRobotAndVerticesAreDemNodes) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + const auto a = mesher.extract(-50.0, 0.0, params()); + const auto b = mesher.extract(50.0, 0.0, params()); + EXPECT_NE(a.triangle_count() + a.x.size(), 0u); + // vertices at the same place keep exact DEM heights + for (const auto * m : {&a, &b}) { + for (std::size_t v = 0; v < m->x.size(); ++v) { + const int i = static_cast(std::lround(m->x[v] / 2.0)) + 32; + const int j = static_cast(std::lround(m->y[v] / 2.0)) + 32; + ASSERT_FLOAT_EQ(m->z[v], dem.at(i, j)); + } + } + // each mesh is denser around its own robot position + auto count_near = [](const LodMesh & m, double rx) { + int n = 0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + if (std::abs(m.x[m.tri[3 * t]] - rx) < 15.0) {++n;} + } + return n; + }; + EXPECT_GT(count_near(a, -50.0), count_near(b, -50.0)); + EXPECT_GT(count_near(b, 50.0), count_near(a, 50.0)); +} + +TEST(LodMesher, NonPowerOfTwoDemIsPaddedAndCulled) +{ + auto dem = make_dem(); + // 41x41 real nodes inside a 65 tile: crop + bigmap_maps_manager::DemGrid g; + g.side = 41; + g.resolution = 2.0; + g.z.resize(41 * 41); + for (int j = 0; j < 41; ++j) { + for (int i = 0; i < 41; ++i) { + g.z[j * 41 + i] = dem.at(i + 12, j + 12); + } + } + const LodMesher mesher(g); + EXPECT_EQ(mesher.error_map().tile_size(), 64); + const auto m = mesher.extract(0.0, 0.0, params()); + double area = 0.0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + area += 0.5 * ((m.x[b] - m.x[a]) * (m.y[c] - m.y[a]) - (m.y[b] - m.y[a]) * (m.x[c] - m.x[a])); + } + EXPECT_GE(area, 80.0 * 80.0 - 1e-3); // real domain fully covered (may overhang the padding) +} + +TEST(LodMesher, LargeDemWithNearFineFarCoarsePolicy) +{ + // 1001 x 1001 nodes at 1 m (padded to a 1025 tile): the production-size case + bigmap_maps_manager::DemGrid g; + g.side = 1001; + g.resolution = 1.0; + g.radius = 500.0; + g.z.resize(static_cast(g.side) * g.side); + for (int j = 0; j < g.side; ++j) { + for (int i = 0; i < g.side; ++i) { + const double x = g.x(i), y = g.y(j); + g.z[static_cast(j) * g.side + i] = static_cast( + 15.0 * std::sin(x / 80.0) * std::cos(y / 60.0) + 0.3 * std::sin(x / 3.0 + y / 2.0)); + } + } + const LodMesher mesher(g); + EXPECT_EQ(mesher.error_map().tile_size(), 1024); + + LodParams p; // the defaults: 1 m within 10 m ... 32 m beyond 200 m + const auto m = mesher.extract(0.0, 0.0, p); + EXPECT_GT(m.triangle_count(), 500u); + EXPECT_LT(m.triangle_count(), 60000u); // vs 2 million at full resolution + + double near_max = 0.0, far_min = 1e9; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const double d = std::hypot((m.x[a] + m.x[b] + m.x[c]) / 3.0, (m.y[a] + m.y[b] + m.y[c]) / 3.0); + if (d < 5.0) {near_max = std::max(near_max, m.leg[t]);} + if (d > 300.0) {far_min = std::min(far_min, m.leg[t]);} + } + EXPECT_LE(near_max, 1.0 + 1e-4); // full resolution next to the robot + EXPECT_GT(far_min, 8.0); // coarse far away (cap there: 32 m) +} + +TEST(LodMesher, ErrorDrivenMeshStaysWithinToleranceAtEveryDemNode) +{ + // With a pure error criterion no DEM node is far from the plane of the triangle that covers it. + // RTIN applies the tolerance to the error at each hypotenuse midpoint (propagated up from its + // descendants), so the error at other nodes is not strictly bounded by it: in practice up to + // ~1.5x. A wrong error map gives a much larger error (it was 9 m for any tolerance). + const auto dem = make_dem(); + const LodMesher mesher(dem); + LodParams p; + p.high_res_radius = 0.0; + p.low_res_distance = 0.0; // beyond high_res: size cap = max_size + p.min_size = dem.resolution; + p.max_size = 1e9; + p.error_growth = 0.0; + for (double tol : {0.3, 1.0, 2.5}) { + p.base_error = tol; + const auto m = mesher.extract(1e6, 1e6, p); // robot far away: only the error decides + ASSERT_LT(m.triangle_count(), 2u * 64 * 64) << "tolerance " << tol << " did not simplify"; + double worst = 0.0; + std::size_t covered = 0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const double x0 = m.x[a], y0 = m.y[a], x1 = m.x[b], y1 = m.y[b], x2 = m.x[c], y2 = m.y[c]; + const double det = (y1 - y2) * (x0 - x2) + (x2 - x1) * (y0 - y2); + const int i0 = static_cast(std::floor((std::min({x0, x1, x2}) + 64.0) / 2.0)); + const int i1 = static_cast(std::ceil((std::max({x0, x1, x2}) + 64.0) / 2.0)); + const int j0 = static_cast(std::floor((std::min({y0, y1, y2}) + 64.0) / 2.0)); + const int j1 = static_cast(std::ceil((std::max({y0, y1, y2}) + 64.0) / 2.0)); + for (int j = std::max(j0, 0); j <= std::min(j1, 64); ++j) { + for (int i = std::max(i0, 0); i <= std::min(i1, 64); ++i) { + const double px = dem.x(i), py = dem.y(j); + const double l0 = ((y1 - y2) * (px - x2) + (x2 - x1) * (py - y2)) / det; + const double l1 = ((y2 - y0) * (px - x2) + (x0 - x2) * (py - y2)) / det; + const double l2 = 1.0 - l0 - l1; + if (l0 < -1e-9 || l1 < -1e-9 || l2 < -1e-9) {continue;} + const double z = l0 * m.z[a] + l1 * m.z[b] + l2 * m.z[c]; + worst = std::max(worst, std::abs(z - dem.at(i, j))); + ++covered; + } + } + } + EXPECT_GT(covered, 65u * 65u); // every node covered (some by several triangles on edges) + EXPECT_LE(worst, 1.75 * tol + 1e-4) << "tolerance " << tol << ", worst " << worst; + } +} + +// --------------------------------------------------------------------------------------------- +// Dynamic layer: what the sensors see raises the NavCels they fall in + +#include "easynav_bigmap_maps_manager/dynamic_layer.hpp" + +using bigmap_maps_manager::DynamicLayer; +using bigmap_maps_manager::DynamicParams; + +namespace +{ +// the unit tests below feed single points: no vertical extent can be asked of them +DynamicParams NoExtent() +{ + DynamicParams p; + p.min_vertical_extent = 0.0; + p.confirm_time = 0.0; + return p; +} + +// the default rules, but taking a single frame as enough +DynamicParams Instant() +{ + DynamicParams p; + p.confirm_time = 0.0; + return p; +} +} // namespace +using bigmap_maps_manager::TriangleLocator; + +TEST(DynamicLayer, LocatorFindsTheTriangleOfEveryPoint) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + const auto m = mesher.extract(0.0, 0.0, params()); + const TriangleLocator loc(m); + for (double x = -63.3; x < 64.0; x += 9.7) { + for (double y = -63.9; y < 64.0; y += 11.3) { + const int t = loc.locate(x, y); + ASSERT_GE(t, 0) << x << ", " << y; + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const double det = (m.y[b] - m.y[c]) * (m.x[a] - m.x[c]) + (m.x[c] - m.x[b]) * + (m.y[a] - m.y[c]); + const double l0 = ((m.y[b] - m.y[c]) * (x - m.x[c]) + (m.x[c] - m.x[b]) * (y - m.y[c])) / det; + const double l1 = ((m.y[c] - m.y[a]) * (x - m.x[c]) + (m.x[a] - m.x[c]) * (y - m.y[c])) / det; + EXPECT_GE(l0, -1e-6); + EXPECT_GE(l1, -1e-6); + EXPECT_GE(1.0 - l0 - l1, -1e-6); + } + } + EXPECT_EQ(loc.locate(500.0, 0.0), -1); +} + +TEST(DynamicLayer, RaisesTheNavCelToWhatTheSensorMeasuresRelativeToTheRobot) +{ + const auto dem = make_dem(); + const LodMesher mesher(dem); + const auto m = mesher.extract(0.0, 0.0, params()); + const TriangleLocator loc(m); + // flat terrain: only what the sensor measured decides the height + const DynamicLayer::Surface terrain = [](double, double) {return 2.0;}; + DynamicParams p = NoExtent(); + + DynamicLayer layer; + // robot at (10, -6) facing +y (yaw 90 deg); an object 3 m ahead: in the robot frame x = 3, and + // 0.8..1.2 m above its ground. In the map that is (10, -3). + std::vector> pts = {{3.0f, 0.0f, 0.8f}, {3.0f, 0.05f, 1.2f}, + {3.1f, 0.0f, 1.0f}}; + EXPECT_TRUE(layer.ingest(pts, 10.0, -6.0, M_PI / 2.0, 0.0, terrain, p)); + const auto tiles = layer.tiles(m, loc); + ASSERT_GE(tiles.size(), 1u); + + const double ground = 2.0; + const int expected_tri = loc.locate(10.0, -3.0); + bool found = false; + for (const auto & t : tiles) { + if (static_cast(t.triangle) == expected_tri) { + found = true; + // the highest detection (1.2 m above the robot's ground), unless the terrain there is higher + const auto a = m.tri[3 * t.triangle], b = m.tri[3 * t.triangle + 1], + c = m.tri[3 * t.triangle + 2]; + const float highest = std::max({m.z[a], m.z[b], m.z[c]}); + EXPECT_NEAR(t.z_top, std::max(ground + 1.2, highest), 1e-3); + EXPECT_GT(t.above_surface, 0.0f); + } + } + EXPECT_TRUE(found) << "the NavCel under the object was not raised"; +} + +TEST(DynamicLayer, IgnoresTheGroundAndFarPointsAndForgetsWhatIsNotSeenAgain) +{ + const DynamicLayer::Surface terrain = [](double, double) {return 0.0;}; + DynamicParams p = NoExtent(); + p.forget_time = 2.0; + DynamicLayer layer; + // ground points (relative to the terrain), too low, and too far + std::vector> ground; + for (float x = 0.5f; x < 4.0f; x += 0.3f) { + ground.push_back({x, 0.0f, 0.0f}); + } + ground.push_back({2.0f, 1.0f, 0.1f}); // 0.1 m above: below min_height + ground.push_back({20.0f, 0.0f, 1.0f}); // 20 m away: beyond max_range + ground.push_back({2.0f, 0.0f, 5.0f}); // above max_height + EXPECT_FALSE(layer.ingest(ground, 0.0, 0.0, 0.0, 0.0, terrain, p)); + EXPECT_EQ(layer.size(), 0u); + + EXPECT_TRUE(layer.ingest({{2.0f, 0.0f, 1.0f}}, 0.0, 0.0, 0.0, 10.0, terrain, p)); + EXPECT_EQ(layer.size(), 1u); + EXPECT_FALSE(layer.expire(11.0, p)); // seen 1 s ago + layer.ingest({{2.0f, 0.0f, 1.0f}}, 0.0, 0.0, 0.0, 11.5, terrain, p); // seen again: refreshed + EXPECT_FALSE(layer.expire(13.0, p)); + EXPECT_TRUE(layer.expire(14.0, p)); // 2.5 s without seeing it + EXPECT_EQ(layer.size(), 0u); +} + +TEST(DynamicLayer, HeightsBelowTheReferenceAreNotClippedToZero) +{ + // a pit: the terrain is 6 m below the DEM's reference; an object 1 m high stands on its floor + bigmap_maps_manager::DemGrid g; + g.side = 33; + g.resolution = 2.0; + g.z.assign(33 * 33, -6.0f); + const LodMesher mesher(g); + const auto m = mesher.extract(0.0, 0.0, params()); + const TriangleLocator loc(m); + const DynamicLayer::Surface terrain = [&g](double x, double y) {return g.height_at(x, y);}; + DynamicLayer layer; + ASSERT_TRUE(layer.ingest({{3.0f, 0.0f, 1.0f}}, 0.0, 0.0, 0.0, 0.0, terrain, NoExtent())); + const auto tiles = layer.tiles(m, loc); + ASSERT_EQ(tiles.size(), 1u); + EXPECT_NEAR(tiles[0].z_top, -5.0f, 1e-3); // -6 + 1, not clipped to 0 + EXPECT_NEAR(tiles[0].above_surface, 1.0f, 1e-3); +} + +TEST(DynamicLayer, HeightIsMeasuredAlongTheNavCelNormalSoARampIsNotAnObstacle) +{ + // a 26.6 degree ramp, z = 0.5 x (slope 0.5): its NavCels are tilted + bigmap_maps_manager::DemGrid g; + g.side = 65; + g.resolution = 2.0; + g.z.resize(65 * 65); + for (int j = 0; j < 65; ++j) { + for (int i = 0; i < 65; ++i) { + g.z[j * 65 + i] = static_cast(0.5 * g.x(i)); + } + } + const LodMesher mesher(g); + const auto m = mesher.extract(-20.0, 0.0, params()); + const TriangleLocator loc(m); + const DynamicLayer::Surface terrain = [&g](double x, double y) {return g.height_at(x, y);}; + const double cos_slope = 1.0 / std::sqrt(1.0 + 0.25); // 0.894 + + // robot on the ramp at (-20, 0) facing +x; the ramp 3 m ahead is 1.5 m higher + auto count_after = [&](float vertical_offset) { + DynamicLayer layer; + DynamicParams p = NoExtent(); // min_height 0.3 along the normal + layer.ingest( + {{3.0f, 0.0f, 1.5f + vertical_offset}}, -20.0, 0.0, 0.0, 0.0, terrain, p, &m, &loc); + return layer.size(); + }; + EXPECT_EQ(count_after(0.0f), 0u); // the ramp's own surface: not an obstacle + EXPECT_EQ(count_after(0.31f), 0u); // 0.31 m vertical = 0.277 m along the normal: below 0.3 + EXPECT_EQ(count_after(0.40f), 1u); // 0.40 vertical = 0.358 normal: an obstacle + // (the vertical criterion would have accepted the 0.31 m one: 0.31 >= 0.3) + EXPECT_GT(0.31, 0.3); + EXPECT_LT(0.31 * cos_slope, 0.3); + + // and the NavCel is raised to the measured height, absolute, as before + DynamicLayer layer; + layer.ingest({{3.0f, 0.0f, 1.9f}}, -20.0, 0.0, 0.0, 0.0, terrain, NoExtent(), &m, &loc); + const auto tiles = layer.tiles(m, loc); + ASSERT_EQ(tiles.size(), 1u); + // measured: ground at the robot (-10) + 1.9; a NavCel is never lowered below its highest vertex + const auto t0 = tiles[0].triangle; + const double highest = std::max({m.z[m.tri[3 * t0]], m.z[m.tri[3 * t0 + 1]], + m.z[m.tri[3 * t0 + 2]]}); + EXPECT_NEAR(tiles[0].z_top, std::max(0.5 * -20.0 + 1.9, highest), 1e-3); +} + +TEST(DynamicLayer, TheRobotsInclinationIsTakenIntoAccountSoSlopesAreNotObstacles) +{ + // 26.6 degree ramp z = 0.5 x; the robot is on it, pitched up along the slope + bigmap_maps_manager::DemGrid g; + g.side = 65; + g.resolution = 2.0; + g.z.resize(65 * 65); + for (int j = 0; j < 65; ++j) { + for (int i = 0; i < 65; ++i) { + g.z[j * 65 + i] = static_cast(0.5 * g.x(i)); + } + } + const LodMesher mesher(g); + const auto m = mesher.extract(-20.0, 0.0, params()); + const TriangleLocator loc(m); + const DynamicLayer::Surface terrain = [&g](double x, double y) {return g.height_at(x, y);}; + const double slope = std::atan(0.5); + // rotation about +y by -slope: the body x axis points up the ramp (nose up) + const std::array nose_up = {0.0, std::sin(-slope / 2.0), 0.0, std::cos(slope / 2.0)}; + + // the ground seen in the robot's own frame is its x-y plane: ahead, behind and to the sides + const std::vector> ground = { + {3.0f, 0.0f, 0.0f}, {-3.0f, 0.0f, 0.0f}, {0.0f, 2.0f, 0.0f}, {0.0f, -2.0f, 0.0f}, + {4.0f, 1.0f, 0.0f}, {-4.0f, -1.0f, 0.0f}}; + + DynamicLayer tilted; + tilted.ingest(ground, -20.0, 0.0, nose_up, 0.0, terrain, NoExtent(), &m, &loc); + EXPECT_EQ(tilted.size(), 0u) << "the ramp the robot is on was taken for obstacles"; + + // treating the same points as if the robot were level (what was done before) invents obstacles + DynamicLayer level; + level.ingest(ground, -20.0, 0.0, 0.0, 0.0, terrain, NoExtent(), &m, &loc); + EXPECT_GT(level.size(), 0u); + + // a real object on the ramp: 0.45 m above its surface, along the normal (the robot's z axis) + DynamicLayer object; + object.ingest({{3.0f, 0.0f, 0.45f}}, -20.0, 0.0, nose_up, 0.0, terrain, NoExtent(), &m, + &loc); + EXPECT_EQ(object.size(), 1u); + DynamicLayer small; + small.ingest({{3.0f, 0.0f, 0.20f}}, -20.0, 0.0, nose_up, 0.0, terrain, NoExtent(), &m, &loc); + EXPECT_EQ(small.size(), 0u); // 0.2 m: below the 0.3 m of the YAML +} + +TEST(DynamicLayer, GroundTheTerrainModelPutsTooLowIsNotAnObstacleButAWallIs) +{ + // flat terrain at z = 0, but the sensor sees the real ground 0.45 m higher (a DEM that is a few + // decimetres off, as every DEM is): a flat patch of ground, then a wall (an object) + bigmap_maps_manager::DemGrid g; + g.side = 65; + g.resolution = 2.0; + g.z.assign(65 * 65, 0.0f); + const LodMesher mesher(g); + const auto m = mesher.extract(0.0, 0.0, params()); + const TriangleLocator loc(m); + const DynamicLayer::Surface terrain = [](double, double) {return 0.0;}; + + std::vector> high_ground; + for (float x = 1.0f; x < 4.0f; x += 0.1f) { + for (float y = -1.0f; y < 1.0f; y += 0.1f) { + high_ground.push_back({x, y, 0.45f + 0.02f * std::sin(10.0f * x + 7.0f * y)}); + } + } + DynamicLayer ground; + ground.ingest(high_ground, 0.0, 0.0, 0.0, 0.0, terrain, Instant(), &m, &loc); + EXPECT_EQ(ground.size(), 0u) << "flat ground 0.45 m above the model was taken for obstacles"; + + // without the extent rule it would be (the height rule alone cannot tell them apart) + DynamicLayer naive; + DynamicParams no_rules = NoExtent(); + no_rules.ground_fit = false; + naive.ingest(high_ground, 0.0, 0.0, 0.0, 0.0, terrain, no_rules, &m, &loc); + EXPECT_GT(naive.size(), 0u); + + // a wall: its face spans from the ground (0.45) up to 1.45 + std::vector> wall; + for (float z = 0.45f; z <= 1.45f; z += 0.05f) { + wall.push_back({3.0f, 0.0f, z}); + } + DynamicLayer obstacle; + obstacle.ingest(wall, 0.0, 0.0, 0.0, 0.0, terrain, Instant(), &m, &loc); + EXPECT_GE(obstacle.size(), 1u); + + // a ramp's own surface on a 26 degree slope: within one 0.25 m cell it spans 0.11 m < 0.2 m + std::vector> slope_ground; + for (float x = 1.0f; x < 4.0f; x += 0.05f) { + slope_ground.push_back({x, 0.0f, 0.5f * x + 0.5f}); + } + DynamicLayer slope; + slope.ingest(slope_ground, 0.0, 0.0, 0.0, 0.0, terrain, Instant(), &m, &loc); + EXPECT_EQ(slope.size(), 0u) << "the extent of a steep ground cell must stay below the rule"; +} + +TEST(DynamicLayer, TheSensorsOwnGroundPlaneKeepsARampFromBeingAnObstacleEvenIfTheMapIsWrong) +{ + // the map says flat, the sensor sees a 30 degree ramp ahead: with ground_fit the ramp is the + // plane the points lie on; a box standing on it is not + bigmap_maps_manager::DemGrid g; + g.side = 65; + g.resolution = 2.0; + g.z.assign(65 * 65, 0.0f); + const LodMesher mesher(g); + const auto m = mesher.extract(0.0, 0.0, params()); + const TriangleLocator loc(m); + const DynamicLayer::Surface terrain = [](double, double) {return 0.0;}; + const float k = std::tan(30.0f * static_cast(M_PI) / 180.0f); + + std::vector> ramp; + for (float x = 0.6f; x < 4.0f; x += 0.05f) { + for (float y = -1.2f; y < 1.2f; y += 0.05f) { + ramp.push_back({x, y, k * x}); + } + } + DynamicParams p = NoExtent(); // no extent rule: only the plane can save the ramp + p.ground_fit = false; + DynamicLayer naive; + naive.ingest(ramp, 0.0, 0.0, 0.0, 0.0, terrain, p, &m, &loc); + EXPECT_GT(naive.size(), 0u) << "without the fit a wrong map turns the ramp into obstacles"; + + p.ground_fit = true; + DynamicLayer fitted; + fitted.ingest(ramp, 0.0, 0.0, 0.0, 0.0, terrain, p, &m, &loc); + EXPECT_EQ(fitted.size(), 0u) << "the ramp's own plane was taken for obstacles"; + + // a 0.6 m box on the ramp at x = 3 + auto with_box = ramp; + for (float z = 0.0f; z <= 0.6f; z += 0.05f) { + for (float y = -0.2f; y <= 0.2f; y += 0.05f) { + with_box.push_back({3.0f, y, k * 3.0f + z}); + } + } + DynamicLayer boxed; + boxed.ingest(with_box, 0.0, 0.0, 0.0, 0.0, terrain, p, &m, &loc); + EXPECT_GE(boxed.size(), 1u); +} + +TEST(DynamicLayer, ADetectionNeedsToPersistInTheSameCellToBeConfirmed) +{ + bigmap_maps_manager::DemGrid g; + g.side = 33; + g.resolution = 2.0; + g.z.assign(33 * 33, 0.0f); + const LodMesher mesher(g); + const auto m = mesher.extract(0.0, 0.0, params()); + const TriangleLocator loc(m); + const DynamicLayer::Surface terrain = [](double, double) {return 0.0;}; + DynamicParams p; // confirm_time 0.3 s, the extent rule on + std::vector> wall; + for (float z = 0.0f; z <= 1.0f; z += 0.05f) { + wall.push_back({3.0f, 0.0f, z}); + } + + // a real object: seen again and again in the same place + DynamicLayer object; + object.ingest(wall, 0.0, 0.0, 0.0, 0.0, terrain, p, &m, &loc); + EXPECT_FALSE(object.expire(0.05, p)); + EXPECT_TRUE(object.tiles(m, loc).empty()) << "confirmed after a single frame"; + object.ingest(wall, 0.0, 0.0, 0.0, 0.2, terrain, p, &m, &loc); + object.ingest(wall, 0.0, 0.0, 0.0, 0.4, terrain, p, &m, &loc); + EXPECT_TRUE(object.expire(0.4, p)); // confirmed now + EXPECT_FALSE(object.tiles(m, loc).empty()); + + // a glitch: a different cell every frame + DynamicLayer glitch; + for (int k = 0; k < 6; ++k) { + std::vector> fleeting; + for (float z = 0.0f; z <= 1.0f; z += 0.05f) { + fleeting.push_back({2.0f + 0.5f * k, 0.0f, z}); + } + glitch.ingest(fleeting, 0.0, 0.0, 0.0, 0.1 * k, terrain, p, &m, &loc); + glitch.expire(0.1 * k, p); + EXPECT_TRUE(glitch.tiles(m, + loc).empty()) << "a fleeting detection was confirmed (frame " << k << ")"; + } + glitch.expire(2.0, p); + EXPECT_EQ(glitch.size(), 0u); // and the unconfirmed ones are dropped quickly +} + +namespace +{ +// 129x129 nodes, 1 m apart, flat, with two walls (8 m high) 2 m apart running along x at y = 0..2 +// (a corridor, narrow) and a hall 40 m wide next to it (no walls near) +bigmap_maps_manager::DemGrid corridor_dem() +{ + bigmap_maps_manager::DemGrid g; + g.side = 129; + g.resolution = 1.0; + g.radius = 64.0; + g.z.assign(129 * 129, 0.0f); + for (int j = 0; j < 129; ++j) { + for (int i = 0; i < 129; ++i) { + const double y = g.y(j), x = g.x(i); + if (x > -60.0 && x < 0.0 && (std::abs(y - 4.0) < 0.1 || std::abs(y - 7.0) < 0.1)) { + g.z[j * 129 + i] = 8.0f; // two walls, 3 m apart; between them 2 free nodes + } + } + } + return g; +} + +std::size_t triangles_in(const LodMesh & m, double x0, double x1, double y0, double y1) +{ + std::size_t n = 0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + double cx = 0, cy = 0; + for (int k = 0; k < 3; ++k) { + cx += m.x[m.tri[3 * t + k]] / 3; cy += m.y[m.tri[3 * t + k]] / 3; + } + if (cx > x0 && cx < x1 && cy > y0 && cy < y1) {++n;} + } + return n; +} +} // namespace + +TEST(LodMesher, NarrowPassagesKeepTheGridResolutionFarFromTheRobot) +{ + const auto dem = corridor_dem(); + LodParams p = params(); + p.min_size = 1.0; + p.high_res_radius = 5.0; + p.low_res_distance = 40.0; + p.error_growth = 2.0; // far geometry is simplified at once + const auto pinned = bigmap_maps_manager::narrow_passage_nodes(dem, {}, 35.0, 5.0); + ASSERT_FALSE(pinned.empty()); + std::size_t n_pinned = 0; + for (auto v : pinned) { + n_pinned += v; + } + EXPECT_GT(n_pinned, 50u); + // robot at the east end, the corridor (x -60..0) is 20..60 m away + const LodMesher plain(dem), kept(dem, {}, pinned); + const auto a = plain.extract(60.0, -40.0, p); + const auto b = kept.extract(60.0, -40.0, p); + EXPECT_GT(triangles_in(b, -60, -5, 3, 8), 3 * triangles_in(a, -60, -5, 3, 8) + 50); + // the open ground far from the passage is not refined + EXPECT_LE(triangles_in(b, 20, 60, -64, -30), triangles_in(a, 20, 60, -64, -30) + 8); + // still crack-free and manifold + for (const auto & kv : edge_use(b)) { + EXPECT_LE(kv.second, 2); + } + EXPECT_EQ(bigmap_maps_manager::narrow_passage_nodes(dem, {}, 35.0, 0.0).size(), 0u); +} + +TEST(LodMesher, TheFullResolutionStripAroundDrivenOverGroundIsKnownMarginWide) +{ + // with the falloff the node uses ((low_res - full_res) / traversed_fade, ~40 here) the strip is + // still known_margin wide (it used to be known_margin / known_falloff: a few centimetres) + const auto dem = make_dem(); + const LodMesher mesher(dem); + bigmap_maps_manager::KnownMap known(dem.side, dem.resolution); + for (double x = -60.0; x <= -20.0; x += 1.0) { + known.mark(x, -40.0, 1.0); + } + known.update(); + LodParams p = params(); + p.known_falloff = 40.0; + auto leaf_near = [&](const LodMesh & m, double x, double y) { + double best = 1e9, leg = -1.0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + double cx = 0, cy = 0; + for (int k = 0; k < 3; ++k) { + cx += m.x[m.tri[3 * t + k]] / 3; + cy += m.y[m.tri[3 * t + k]] / 3; + } + const double d = std::hypot(cx - x, cy - y); + if (d < best) {best = d; leg = m.leg[t];} + } + return leg; + }; + p.known_margin = 12.0; + const auto wide = mesher.extract(50.0, 50.0, p, &known); + p.known_margin = 1.0; + const auto narrow = mesher.extract(50.0, 50.0, p, &known); + // 10 m from the trail (2 m DEM: the crack-free slack is kReach x hypotenuse, ~3.5 m): inside the + // 12 m strip, well outside the 1 m one + EXPECT_DOUBLE_EQ(leaf_near(wide, -40.0, -50.0), p.min_size); + EXPECT_GT(leaf_near(narrow, -40.0, -50.0), p.min_size); + EXPECT_GT(wide.triangle_count(), narrow.triangle_count()); + for (const auto & kv : edge_use(wide)) { + EXPECT_LE(kv.second, 2); + } +} + +namespace +{ +// T-junctions: a vertex of the mesh lying strictly inside an edge used by one triangle only (a +// crack). Vertices are DEM nodes, so the lattice points along each such edge are checked. +std::size_t t_junctions(const LodMesh & m, double res) +{ + std::set> nodes; // NOLINT(runtime/int) + auto node = [&](std::uint32_t v) { + return std::make_pair(std::lround(m.x[v] / res), std::lround(m.y[v] / res)); + }; + for (std::uint32_t v = 0; v < m.x.size(); ++v) { + nodes.insert(node(v)); + } + std::size_t bad = 0; + for (const auto & [e, n] : edge_use(m)) { + if (n != 1) {continue;} + const auto a = node(e.first), b = node(e.second); + const long dx = b.first - a.first, dy = b.second - a.second; // NOLINT(runtime/int) + const long g = std::gcd(std::labs(dx), std::labs(dy)); // NOLINT(runtime/int) + for (long k = 1; k < g; ++k) { // NOLINT(runtime/int) + if (nodes.count({a.first + dx / g * k, a.second + dy / g * k})) {++bad;} + } + } + return bad; +} + +bigmap_maps_manager::DemGrid rough_dem(int side) +{ + bigmap_maps_manager::DemGrid g; + g.side = side; + g.resolution = 2.0; + g.radius = (side - 1) / 2 * 2.0; + g.z.resize(static_cast(side) * side); + for (int j = 0; j < side; ++j) { + for (int i = 0; i < side; ++i) { + const double x = g.x(i), y = g.y(j); + g.z[static_cast(j) * side + i] = static_cast( + 8.0 * std::sin(x / 17.0) * std::cos(y / 13.0) + 0.6 * std::sin(x / 3.1 + y / 2.3)); + } + } + return g; +} +} // namespace + +TEST(LodMesher, NoCracksWithDrivenOverGroundOnGridsThatAreNotTwoToTheKPlusOne) +{ + // the RTIN pads these DEMs to 65 nodes; the driven-over field must cover that padding with a + // true distance, or the policy stops being monotone there and the mesh cracks. A sweep of + // trails, policies and robot positions (deterministic) + std::size_t cases = 0, bad = 0; + for (const int side : {41, 45, 51, 61, 65}) { + const auto dem = rough_dem(side); + const LodMesher mesher(dem); + const double edge = (side - 1) / 2 * 2.0; + for (int trail = 0; trail < 3; ++trail) { + bigmap_maps_manager::KnownMap known(dem.side, dem.resolution, mesher.error_map().grid_size()); + for (double t = -edge; t <= edge; t += 1.0) { + if (trail == 0) {known.mark(t, 0.3 * t, 1.0);} // across the map + if (trail == 1) {known.mark(edge - 1.0, t, 1.0);} // along the high-side edge + if (trail == 2) {known.mark(t, edge - 1.0, 1.0);} + } + known.update(); + for (const double falloff : {1.0, 3.0, 8.0, 40.0}) { + for (const double margin : {0.5, 3.0, 9.0}) { + LodParams p = params(); + p.high_res_radius = 6.0; + p.low_res_distance = 40.0; + p.known_falloff = falloff; + p.known_margin = margin; + for (double rx = -90.0; rx <= 90.0; rx += 45.0) { + const auto m = mesher.extract(rx, 0.7 * rx - 20.0, p, &known); + const auto n = t_junctions(m, dem.resolution); + ++cases; + bad += n > 0; + EXPECT_EQ(n, 0u) << "side " << side << " trail " << trail << " falloff " << falloff + << " margin " << margin << " robot x " << rx; + } + } + } + } + } + EXPECT_EQ(bad, 0u) << "of " << cases << " meshes"; +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_main_piece.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_main_piece.cpp new file mode 100644 index 00000000..cde2b9b9 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_main_piece.cpp @@ -0,0 +1,107 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include + +#include +#include +#include + +#include "easynav_bigmap_maps_manager/dem_grid.hpp" +#include "easynav_bigmap_maps_manager/narrow_passages.hpp" + +using bigmap_maps_manager::keep_main_piece; + +namespace +{ +constexpr int kSide = 30; + +bigmap_maps_manager::DemGrid flat(float z = 0.0f) +{ + bigmap_maps_manager::DemGrid d; + d.side = kSide; + d.resolution = 1.0; + d.z.assign(static_cast(kSide) * kSide, z); + return d; +} + +std::size_t at(int x, int y) {return static_cast(y) * kSide + x;} +} // namespace + +TEST(MainPiece, TheRoofOfABuildingIsDroppedAndItsWallsStay) +{ + auto d = flat(); + for (int y = 10; y < 20; ++y) { + for (int x = 10; x < 20; ++x) { + d.z[at(x, y)] = 20.0f; + } // a 10 x 10 m block, 20 m high + } + std::size_t dropped = 0; + const auto v = keep_main_piece(d, {}, 25.0, &dropped); + EXPECT_TRUE(v[at(2, 2)]); // the street + EXPECT_TRUE(v[at(9, 15)]); // the foot of the wall + EXPECT_TRUE(v[at(10, 15)]); // the wall's top edge: the obstacle itself stays + EXPECT_FALSE(v[at(15, 15)]); // the roof: no goal can be picked there + EXPECT_FALSE(v[at(12, 12)]); + EXPECT_EQ(dropped, 64u); // the 8 x 8 inner nodes of the roof +} + +TEST(MainPiece, GroundThatIsReachableByARampIsKept) +{ + auto d = flat(); + // a platform 3 m up and a ramp to it (26 deg) + for (int y = 10; y < 20; ++y) { + for (int x = 10; x < 20; ++x) { + d.z[at(x, y)] = 3.0f; + } + for (int x = 4; x < 10; ++x) { + d.z[at(x, y)] = 0.5f * (x - 3); + } + } + std::size_t dropped = 0; + const auto v = keep_main_piece(d, {}, 35.0, &dropped); + EXPECT_TRUE(v[at(15, 15)]); // on the platform: the robot can drive there + EXPECT_EQ(dropped, 0u); + // the same platform with no ramp: it is a roof + for (int y = 10; y < 20; ++y) { + for (int x = 4; x < 10; ++x) { + d.z[at(x, y)] = 0.0f; + } + } + EXPECT_FALSE(keep_main_piece(d, {}, 35.0)[at(15, 15)]); +} + +TEST(MainPiece, OnlyTheLargestPieceIsKept) +{ + auto d = flat(); + for (int y = 0; y < kSide; ++y) { + d.z[at(15, y)] = -30.0f; + } // a trench splits the map + const auto v = keep_main_piece(d, {}, 25.0); + EXPECT_TRUE(v[at(3, 3)]); // the larger side (x < 14) + EXPECT_FALSE(v[at(25, 20)]); // the smaller one +} + +TEST(MainPiece, NodesWithNoDataStayWithNoData) +{ + auto d = flat(); + std::vector valid(d.z.size(), 1); + valid[at(5, 5)] = 0; + std::size_t dropped = 0; + const auto v = keep_main_piece(d, valid, 25.0, &dropped); + EXPECT_FALSE(v[at(5, 5)]); + EXPECT_EQ(dropped, 0u); // it had none to lose + EXPECT_TRUE(v[at(6, 5)]); +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_navmap_dem.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_navmap_dem.cpp new file mode 100644 index 00000000..30e36f0f --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_navmap_dem.cpp @@ -0,0 +1,435 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/known_map.hpp" +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" +#include "easynav_bigmap_maps_manager/navmap_dem.hpp" +#include "navmap_core/NavMap.hpp" + +using bigmap_maps_manager::dem_from_navmap; + +namespace +{ +double plane(double x, double y) {return 0.3 * x + 0.1 * y + 2.0;} + +// Triangles of leg `h` over [x0, x1] x [y0, y1] (an L-shape if `cut`), on the plane above, +// coloured by x; the NavCels with x < -10 are `traversed` +::navmap::NavMap make_map(double h, bool cut, bool with_traversed) +{ + ::navmap::NavMap nm; + const int nx = static_cast(std::ceil(60.0 / h)), ny = static_cast(std::ceil(40.0 / h)); + ::navmap::Colors col; + auto vid = [&](int i, int j) {return static_cast(j * (nx + 1) + i);}; + for (int j = 0; j <= ny; ++j) { + for (int i = 0; i <= nx; ++i) { + const double x = -30.0 + i * h, y = -20.0 + j * h; + nm.positions.x.push_back(static_cast(x)); + nm.positions.y.push_back(static_cast(y)); + nm.positions.z.push_back(static_cast(plane(x, y))); + col.r.push_back(static_cast(128 + 2 * x)); + col.g.push_back(50); + col.b.push_back(200); + col.a.push_back(255); + } + } + nm.colors = col; + std::vector cx; + for (int j = 0; j < ny; ++j) { + for (int i = 0; i < nx; ++i) { + const double x = -30.0 + (i + 0.5) * h, y = -20.0 + (j + 0.5) * h; + if (cut && x > 0.0 && y > 0.0) {continue;} // L shape: the upper right quarter is missing + for (int k = 0; k < 2; ++k) { + ::navmap::NavCel c; + if (k == 0) { + c.v[0] = vid(i, j); c.v[1] = vid(i + 1, j); c.v[2] = vid(i + 1, j + 1); + } else { + c.v[0] = vid(i, j); c.v[1] = vid(i + 1, j + 1); c.v[2] = vid(i, j + 1); + } + nm.navcels.push_back(c); + cx.push_back(x); + } + } + } + nm.surfaces.resize(1); + nm.surfaces[0].navcels.resize(nm.navcels.size()); + for (std::size_t t = 0; t < nm.navcels.size(); ++t) { + nm.surfaces[0].navcels[t] = static_cast<::navmap::NavCelId>(t); + } + if (with_traversed) { + nm.add_layer("traversed", "", "", 0); + for (std::size_t t = 0; t < nm.navcels.size(); ++t) { + if (cx[t] < -10.0) { + nm.layer_set("traversed", static_cast<::navmap::NavCelId>(t), 255); + } + } + } + return nm; +} +} // namespace + +TEST(NavMapDem, HeightsAndColoursAreResampledOnTheGridAtAnyNavCelSize) +{ + for (double h : {0.5, 3.0, 7.5}) { + const auto nd = dem_from_navmap(make_map(h, false, false), 1.0); + const auto & d = nd.dem; + EXPECT_EQ(d.side % 2, 1); + int checked = 0; + for (double x = -29.0; x <= 29.0; x += 2.0) { + for (double y = -19.0; y <= 19.0; y += 2.0) { + const int i = static_cast(std::lround(x / d.resolution)) + (d.side - 1) / 2; + const int j = static_cast(std::lround(y / d.resolution)) + (d.side - 1) / 2; + ASSERT_TRUE(nd.valid[j * d.side + i]) << "h " << h << " at " << x << "," << y; + EXPECT_NEAR(d.at(i, j), plane(x, y), 2e-3); + EXPECT_NEAR(d.rgb[3 * (j * d.side + i)], 128 + 2 * x, 1.5); + ++checked; + } + } + EXPECT_GT(checked, 100); + } +} + +TEST(NavMapDem, GroundOutsideTheMapIsNoDataAndTheMesherDropsIt) +{ + const auto nd = dem_from_navmap(make_map(2.0, true, false), 1.0); + const auto & d = nd.dem; + auto idx = [&](double x, double y) { + return (static_cast(std::lround(y)) + (d.side - 1) / 2) * d.side + + static_cast(std::lround(x)) + (d.side - 1) / 2; + }; + EXPECT_TRUE(nd.valid[idx(-10, 10)]); + EXPECT_FALSE(nd.valid[idx(15, 10)]); // the missing quarter + EXPECT_TRUE(std::isfinite(d.z[idx(15, 10)])); // but filled, never an infinity + + const bigmap_maps_manager::LodMesher mesher(d, nd.valid); + bigmap_maps_manager::LodParams p; + p.high_res_radius = 10.0; + p.min_size = 1.0; + p.max_size = 8.0; + const auto m = mesher.extract(-20.0, 0.0, p); + ASSERT_GT(m.triangle_count(), 0u); + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + double cx = 0, cy = 0; + for (int k = 0; k < 3; ++k) { + cx += m.x[m.tri[3 * t + k]] / 3; + cy += m.y[m.tri[3 * t + k]] / 3; + } + EXPECT_FALSE(cx > 3.0 && + cy > 3.0) << "a NavCel over the missing quarter at " << cx << "," << cy; + } +} + +TEST(NavMapDem, TraversedLayerIsRestoredAsKnownGround) +{ + const auto without = dem_from_navmap(make_map(2.0, false, false), 1.0); + EXPECT_FALSE(without.has_traversed); + EXPECT_TRUE(without.traversed_nodes.empty()); + + const auto nd = dem_from_navmap(make_map(2.0, false, true), 1.0); + EXPECT_TRUE(nd.has_traversed); + ASSERT_FALSE(nd.traversed_nodes.empty()); + bigmap_maps_manager::KnownMap known(nd.dem.side, nd.dem.resolution); + for (const auto & [i, j] : nd.traversed_nodes) { + known.mark_node(i, j); + } + known.update(); + const int c = (nd.dem.side - 1) / 2; + EXPECT_TRUE(known.known(c - 20, c)); // x = -20: traversed + EXPECT_FALSE(known.known(c + 10, c)); // x = 10: not +} + +TEST(NavMapDem, OriginFileRoundTripsAndItsAbsenceMeansNoGeolocation) +{ + const std::string dir = std::filesystem::temp_directory_path().string() + "/lmm_origin_test"; + std::filesystem::create_directories(dir); + const std::string map = dir + "/some_map.navmap"; + EXPECT_EQ(bigmap_maps_manager::origin_file_for(map), dir + "/some_map.yaml"); + std::filesystem::remove(dir + "/some_map.yaml"); + + auto none = bigmap_maps_manager::load_origin(map); + EXPECT_FALSE(none.georeferenced); + EXPECT_EQ(none.lat, 0.0); + EXPECT_EQ(none.altitude, 0.0); + + bigmap_maps_manager::MapOrigin o{true, 40.284796, -3.821407, 675.25}; + ASSERT_TRUE(bigmap_maps_manager::save_origin(map, o, 1.0)); + const auto back = bigmap_maps_manager::load_origin(map); + EXPECT_TRUE(back.georeferenced); + EXPECT_NEAR(back.lat, 40.284796, 1e-9); + EXPECT_NEAR(back.lon, -3.821407, 1e-9); + EXPECT_NEAR(back.altitude, 675.25, 1e-9); + + ASSERT_TRUE(bigmap_maps_manager::save_origin(map, bigmap_maps_manager::MapOrigin{}, 1.0)); + EXPECT_FALSE(bigmap_maps_manager::load_origin(map).georeferenced); + // "nan" and "inf" are numbers to std::stod: a georeferenced origin nowhere was accepted + for (const char * text : {"georeferenced: true\norigin_lat: nan\norigin_lon: -3.8\n", + "georeferenced: true\norigin_lat: 40.2\norigin_lon: inf\n", + "georeferenced: true\norigin_lat: 40.2\norigin_lon: -3.8\nreference_altitude: -inf\n"}) + { + std::ofstream(dir + "/some_map.yaml", std::ios::trunc) << text; + EXPECT_FALSE(bigmap_maps_manager::load_origin(map).georeferenced) << text; + } + std::filesystem::remove_all(dir); +} + +TEST(NavMapDem, RefusesAnEmptyMapAndAHugeGrid) +{ + EXPECT_THROW(dem_from_navmap(::navmap::NavMap(), 1.0), std::invalid_argument); + EXPECT_THROW(dem_from_navmap(make_map(2.0, false, false), 0.001), std::invalid_argument); +} + +namespace +{ +// Flat strips of 1 m NavCels over [x0, x1] x [-5, 5] (one per pair), z = 0 +::navmap::NavMap strips(const std::vector> & spans) +{ + ::navmap::NavMap nm; + for (const auto & [x0, x1] : spans) { + const auto base = static_cast(nm.positions.x.size()); + const int nx = x1 - x0; + for (int j = 0; j <= 10; ++j) { + for (int i = 0; i <= nx; ++i) { + nm.positions.x.push_back(static_cast(x0 + i)); + nm.positions.y.push_back(static_cast(j - 5)); + nm.positions.z.push_back(0.0f); + } + } + auto vid = [&](int i, int j) {return base + static_cast(j * (nx + 1) + i);}; + for (int j = 0; j < 10; ++j) { + for (int i = 0; i < nx; ++i) { + ::navmap::NavCel a, b; + a.v[0] = vid(i, j); a.v[1] = vid(i + 1, j); a.v[2] = vid(i + 1, j + 1); + b.v[0] = vid(i, j); b.v[1] = vid(i + 1, j + 1); b.v[2] = vid(i, j + 1); + nm.navcels.push_back(a); + nm.navcels.push_back(b); + } + } + } + return nm; +} + +bool valid_at(const bigmap_maps_manager::NavMapDem & d, int x, int y) +{ + const int c = (d.dem.side - 1) / 2; + return d.valid[static_cast(y + c) * d.dem.side + (x + c)] != 0; +} +} // namespace + +TEST(NavMapDem, BordersDoNotGrowAndAGapOfTwoNodesStays) +{ + // a platform over x 0..4 and another over x 7..10: nodes x = 5, 6 are a gap of 2 nodes + const auto d = dem_from_navmap(strips({{0, 4}, {7, 10}}), 1.0); + EXPECT_EQ(d.covered_nodes, 11u * (5u + 4u)); // exactly the nodes on the NavCels + for (int y = -5; y <= 5; ++y) { + EXPECT_FALSE(valid_at(d, -1, y)) << y; // not past the border + EXPECT_FALSE(valid_at(d, 5, y)) << y; // the gap stays + EXPECT_FALSE(valid_at(d, 6, y)) << y; + EXPECT_FALSE(valid_at(d, 11, y)) << y; + } + // a gap of one node (sampling) is closed + const auto one = dem_from_navmap(strips({{0, 4}, {6, 10}}), 1.0); + for (int y = -4; y <= 4; ++y) { + EXPECT_TRUE(valid_at(one, 5, y)) << y; + } +} + +TEST(NavMapDem, ANaNHeightIsNoData) +{ + auto nm = strips({{0, 4}}); + nm.positions.z[0] = std::nanf(""); // the vertex at (0, -5) + const auto d = dem_from_navmap(nm, 1.0); + for (std::size_t k = 0; k < d.dem.z.size(); ++k) { + EXPECT_TRUE(std::isfinite(d.dem.z[k])) << k; + } +} + +TEST(NavMapDem, AbsurdOrNonFiniteVertexCoordinatesNeitherSizeTheGridNorOverflowItsIndices) +{ + // a vertex at 3e38 m made `ceil(half / resolution)` overflow its cast to int (undefined: + // INT_MIN, a negative side that passed the max_side check), and a NaN one passed every + // comparison of the rasteriser and was cast to an index + const auto reference = dem_from_navmap(strips({{0, 4}}), 1.0); + { + auto nm = strips({{0, 4}}); + nm.positions.x[0] = 3.0e38f; + EXPECT_THROW(dem_from_navmap(nm, 1.0), std::invalid_argument); // too big a grid, said so + } + for (const float bad : {std::nanf(""), std::numeric_limits::infinity(), + -std::numeric_limits::infinity()}) + { + auto nm = strips({{0, 4}}); + nm.positions.x[0] = bad; + nm.positions.y[1] = bad; + const auto d = dem_from_navmap(nm, 1.0); + EXPECT_EQ(d.dem.side, reference.dem.side) << bad; // the grid is sized by the finite ones + for (std::size_t k = 0; k < d.dem.z.size(); ++k) { + ASSERT_TRUE(std::isfinite(d.dem.z[k])) << k; + } + } +} + +namespace +{ +// make_map(1.0) without some cells of 1 m: one alone at (5, 5), one at (-12, 3), a 2 x 2 block at +// (15..16, -8..-7). Those are obstacles of one or two cells. +const std::vector> kHoles{{5, 5}, {-12, 3}, {15, -8}, {16, -8}, {15, -7}, + {16, -7}}; + +::navmap::NavMap holed_map() +{ + auto nm = make_map(1.0, false, false); + std::vector<::navmap::NavCel> kept; + for (const auto & c : nm.navcels) { + double cx = 0, cy = 0; + for (int k = 0; k < 3; ++k) { + cx += nm.positions.x[c.v[k]] / 3.0; + cy += nm.positions.y[c.v[k]] / 3.0; + } + bool hole = false; + for (const auto & [hx, hy] : kHoles) { + hole = hole || (std::floor(cx) == hx && std::floor(cy) == hy); + } + if (!hole) {kept.push_back(c);} + } + nm.navcels = kept; + nm.surfaces[0].navcels.resize(kept.size()); + for (std::size_t t = 0; t < kept.size(); ++t) { + nm.surfaces[0].navcels[t] = static_cast<::navmap::NavCelId>(t); + } + return nm; +} + +// Is the point inside some triangle of the mesh (in xy)? +bool covered(const bigmap_maps_manager::LodMesh & m, double px, double py) +{ + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + const auto side = [&](std::uint32_t u, std::uint32_t v) { + return (m.x[v] - m.x[u]) * (py - m.y[u]) - (m.y[v] - m.y[u]) * (px - m.x[u]); + }; + const double s1 = side(a, b), s2 = side(b, c), s3 = side(c, a); + if ((s1 >= 0 && s2 >= 0 && s3 >= 0) || (s1 <= 0 && s2 <= 0 && s3 <= 0)) {return true;} + } + return false; +} +} // namespace + +TEST(NavMapDem, EveryHoleOfTheMapIsACellWithoutGroundWhateverItsSize) +{ + const auto nd = dem_from_navmap(holed_map(), 1.0); + const int side = nd.dem.side, nc = side - 1, c = (side - 1) / 2; + ASSERT_EQ(nd.cell_valid.size(), static_cast(nc) * nc); + const auto cell = [&](int x, int y) { + return nd.cell_valid[static_cast(y + c) * nc + (x + c)]; + }; + for (const auto & [hx, hy] : kHoles) { + EXPECT_EQ(cell(hx, hy), 0) << hx << "," << hy; + } + EXPECT_EQ(cell(4, 5), 1); + EXPECT_EQ(cell(6, 5), 1); + EXPECT_EQ(cell(14, -8), 1); + EXPECT_EQ(cell(0, 0), 1); + EXPECT_EQ(cell(29, 19), 1); + EXPECT_EQ(cell(-31, 0), 0); // beyond the map +} + +TEST(LodMesher, AtFullResolutionTheHolesOfTheMapAreExactlyWhereTheyAre) +{ + // The node rule (a NavCel exists if the node under its centre has ground) closed every hole + // narrower than three cells: the obstacles of one or two cells were driven through. With the + // cell mask a NavCel exists if the map has ground under its centroid. + const auto nd = dem_from_navmap(holed_map(), 1.0); + bigmap_maps_manager::LodParams p; + p.high_res_radius = 60.0; // all of it at full resolution + p.low_res_distance = 100.0; + p.min_size = 1.0; + p.max_size = 8.0; + const bigmap_maps_manager::LodMesher old_rule(nd.dem, nd.valid); + const bigmap_maps_manager::LodMesher exact(nd.dem, nd.valid, {}, nd.cell_valid); + const auto before = old_rule.extract(0.0, 0.0, p); + const auto now = exact.extract(0.0, 0.0, p); + for (const auto & [hx, hy] : kHoles) { + EXPECT_TRUE(covered(before, hx + 0.5, hy + 0.5)) << "the old rule closed it"; + EXPECT_FALSE(covered(now, hx + 0.5, hy + 0.5)) << hx << "," << hy; + } + // and the ground around them is there + for (const auto & [x, y] : std::vector>{{4.5, 5.5}, {5.5, 6.5}, + {-10.5, 3.5}, {14.5, -7.5}, {0.5, 0.5}}) + { + EXPECT_TRUE(covered(now, x, y)) << x << "," << y; + } +} + +TEST(LodMesher, AHoleStaysOpenFarFromTheRobotWithAHoleErrorOrOnceObserved) +{ + const auto nd = dem_from_navmap(holed_map(), 1.0); + bigmap_maps_manager::LodParams p; + p.high_res_radius = 5.0; + p.low_res_distance = 40.0; + p.min_size = 1.0; + p.max_size = 16.0; + p.base_error = 0.02; + p.error_growth = 0.05; + p.known_margin = 1.0; + p.known_falloff = 4.0; + // the robot 40 m from the hole at (15, -8): a coarse NavCel covers it, and it is not emitted + // only if the cell under its centroid happens to be the hole + const bigmap_maps_manager::LodMesher plain(nd.dem, nd.valid, {}, nd.cell_valid); + EXPECT_TRUE(covered(plain.extract(-25.0, 0.0, p), 15.5, -7.5)); + // a hole error above the tolerance at that distance (0.02 + 0.05 * (40 - 5 - ...)) splits down + // to the hole's border + const bigmap_maps_manager::LodMesher strict(nd.dem, nd.valid, {}, nd.cell_valid, 5.0); + const auto far = strict.extract(-25.0, 0.0, p); + for (const auto & [hx, hy] : kHoles) { + EXPECT_FALSE(covered(far, hx + 0.5, hy + 0.5)) << hx << "," << hy; + } + EXPECT_LT(far.triangle_count(), strict.extract(15.0, -8.0, p).triangle_count() * 4); + // without a hole error, the border observed from near keeps it + bigmap_maps_manager::KnownMap observed(nd.dem.side, 1.0); + for (const auto & [hx, hy] : kHoles) { + observed.mark(hx + 0.5, hy + 0.5, 1.0); + } + observed.update(); + const auto kept = plain.extract(-25.0, 0.0, p, nullptr, &observed); + for (const auto & [hx, hy] : kHoles) { + EXPECT_FALSE(covered(kept, hx + 0.5, hy + 0.5)) << hx << "," << hy; + } +} + +TEST(NavMapDem, TheMeshFromTheArraysOfAMapIsTheMeshFromTheMap) +{ + const auto nm = holed_map(); + const auto a = dem_from_navmap(nm, 1.0); + const auto b = dem_from_navmap(bigmap_maps_manager::SourceMesh::from_navmap(nm), 1.0); + EXPECT_EQ(a.dem.z, b.dem.z); + EXPECT_EQ(a.valid, b.valid); + EXPECT_EQ(a.cell_valid, b.cell_valid); + auto broken = bigmap_maps_manager::SourceMesh::from_navmap(nm); + broken.v2[3] = static_cast(broken.x.size()); + EXPECT_THROW(broken.validate(), std::invalid_argument); + broken = bigmap_maps_manager::SourceMesh::from_navmap(nm); + broken.traversed.assign(2, 1); + EXPECT_THROW(broken.validate(), std::invalid_argument); +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_obstacle_memory.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_obstacle_memory.cpp new file mode 100644 index 00000000..54d2ee7a --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_obstacle_memory.cpp @@ -0,0 +1,146 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "easynav_bigmap_maps_manager/dynamic_layer.hpp" +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" +#include "easynav_bigmap_maps_manager/obstacle_memory.hpp" + +using bigmap_maps_manager::DynamicLayer; +using bigmap_maps_manager::DynamicParams; +using bigmap_maps_manager::ObstacleMemory; +using bigmap_maps_manager::SustainedHit; + +namespace +{ +// a box 0.5 m wide and 1 m high in front of the robot, seen from a robot at the origin +std::vector> box(float x, float y) +{ + std::vector> pts; + for (float dx = 0.0f; dx <= 0.4f; dx += 0.1f) { + for (float dy = 0.0f; dy <= 0.4f; dy += 0.1f) { + for (float z = 0.0f; z <= 1.0f; z += 0.1f) { + pts.push_back({x + dx, y + dy, z}); + } + } + } + return pts; +} + +DynamicParams params() +{ + DynamicParams p; + p.ground_fit = false; + p.confirm_time = 0.3; + p.min_vertical_extent = 0.2; + return p; +} + +const DynamicLayer::Surface flat = [](double, double) {return 0.0;}; +} // namespace + +TEST(ObstacleMemory, ADetectionOverFiveSecondsIsOneHitAndTenSecondsAreTwo) +{ + DynamicLayer layer; + const auto p = params(); + const auto pts = box(2.0f, 0.0f); + std::size_t hits = 0; + for (double t = 0.0; t <= 11.0; t += 0.1) { + layer.ingest(pts, 0.0, 0.0, 0.0, t, flat, p); + layer.expire(t, p); + const auto h = layer.sustained(t, 5.0); + if (t < 4.9) {EXPECT_TRUE(h.empty()) << "t=" << t;} + hits += h.empty() ? 0 : 1; + } + EXPECT_GE(hits, 2u); // at 5 s and at 10 s (each detection cell reports its own periods) +} + +TEST(ObstacleMemory, AGapOverASecondStartsTheCountAgain) +{ + DynamicLayer layer; + const auto p = params(); + const auto pts = box(2.0f, 0.0f); + for (double t = 0.0; t <= 4.0; t += 0.1) { + layer.ingest(pts, 0.0, 0.0, 0.0, t, flat, p); + layer.expire(t, p); + } + // not seen for 2 s (still remembered: forget_time is 5 s), then seen again + for (double t = 6.0; t <= 9.0; t += 0.1) { + layer.ingest(pts, 0.0, 0.0, 0.0, t, flat, p); + layer.expire(t, p); + EXPECT_TRUE(layer.sustained(t, 5.0).empty()) << "t=" << t; // 3 s since the gap + } +} + +TEST(ObstacleMemory, HitsAddUpToMaxHitsAndTheObstacleIsThenFixed) +{ + ObstacleMemory mem(1.0, 5.0, 3); + const std::vector d{{3.2f, 4.1f, 0.5f, 0.0f}, {3.4f, 4.3f, 0.7f, 0.0f}}; + EXPECT_TRUE(mem.add(d, 5.0)); // two detections in one spot: one hit + ASSERT_EQ(mem.cells().size(), 1u); + EXPECT_EQ(mem.cells().begin()->second.hits, 1); + EXPECT_FALSE(mem.fixed(mem.cells().begin()->second)); + EXPECT_FALSE(mem.add(d, 6.0)); // half a period later: the same period + EXPECT_TRUE(mem.add(d, 10.0)); + EXPECT_EQ(mem.cells().begin()->second.hits, 2); + EXPECT_TRUE(mem.add(d, 15.0)); + EXPECT_TRUE(mem.fixed(mem.cells().begin()->second)); + EXPECT_EQ(mem.fixed_count(), 1u); + EXPECT_FALSE(mem.add(d, 20.0)); // saturated + EXPECT_EQ(mem.cells().begin()->second.hits, 3); + mem.clear(); + EXPECT_TRUE(mem.cells().empty()); +} + +TEST(ObstacleMemory, SpotsApartAndFloorsApartAreCountedApart) +{ + ObstacleMemory mem(1.0, 5.0, 3); + mem.add({{0.2f, 0.2f, 0.0f, 0.0f}, {5.2f, 0.2f, 0.0f, 0.0f}, {0.2f, 0.2f, 2.2f, 2.2f}}, 5.0); + EXPECT_EQ(mem.cells().size(), 3u); +} + +TEST(ObstacleMemory, StaggeredDetectionsOfOneSpotDoNotFixItSooner) +{ + // two detection cells of the same 1 m spot, first seen 2.6 s apart: each reports its own + // periods (A at 5, 10, 15 s; B at 7.6, 12.6 s). The spot is fixed at 15 s, not at 12.6 s + ObstacleMemory mem(1.0, 5.0, 3); + const SustainedHit a{3.2f, 4.1f, 0.5f, 0.0f}, b{3.7f, 4.6f, 0.5f, 0.0f}; + mem.add({a}, 5.0); + mem.add({b}, 7.6); + mem.add({a}, 10.0); + mem.add({b}, 12.6); + ASSERT_EQ(mem.cells().size(), 1u); + EXPECT_EQ(mem.cells().begin()->second.hits, 2); + EXPECT_FALSE(mem.fixed(mem.cells().begin()->second)); + mem.add({a}, 15.0); + EXPECT_TRUE(mem.fixed(mem.cells().begin()->second)); +} + +TEST(ObstacleMemory, RestoredCountsCarryOnFromWhereTheSavedMapLeftThem) +{ + ObstacleMemory mem(1.0, 5.0, 3); + mem.restore(3.2, 4.1, 0.5, 2); + mem.restore(9.0, 9.0, 0.0, 99); // beyond max_hits: clamped, fixed + ASSERT_EQ(mem.cells().size(), 2u); + EXPECT_EQ(mem.fixed_count(), 1u); + mem.restore(3.4, 4.3, 0.5, 1); // the same spot: it keeps what it had + EXPECT_EQ(mem.cells().size(), 2u); + // a new detection adds to it at once (the saved hit was long ago) and fixes it + mem.add({{3.2f, 4.1f, 0.5f, 0.5f}}, 5.0); + EXPECT_EQ(mem.fixed_count(), 2u); +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_pipeline_check.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_pipeline_check.cpp new file mode 100644 index 00000000..4110d92a --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_pipeline_check.cpp @@ -0,0 +1,149 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "easynav_bigmap_maps_manager/pipeline_check.hpp" + +using bigmap_maps_manager::check_pipeline; +using bigmap_maps_manager::LoadedFilter; +using bigmap_maps_manager::ObstacleDetector; +using bigmap_maps_manager::parse_detector; +using bigmap_maps_manager::PipelineSettings; + +namespace +{ +const LoadedFilter kObstacleFilter{"obstacle_filter", "obstacles"}; +const LoadedFilter kInflation{"inflation", "inflated_obstacles"}; + +bool mentions(const std::vector & v, const std::string & text) +{ + for (const auto & s : v) { + if (s.find(text) != std::string::npos) { + return true; + } + } + return false; +} +} // namespace + +TEST(PipelineCheck, DetectorNamesAndTheOldOnes) +{ + EXPECT_EQ(parse_detector("bigmap_dynamic"), ObstacleDetector::BigmapDynamic); + EXPECT_EQ(parse_detector("dynamic"), ObstacleDetector::BigmapDynamic); + EXPECT_EQ(parse_detector("navmap_filter"), ObstacleDetector::NavmapFilter); + EXPECT_EQ(parse_detector("filter"), ObstacleDetector::NavmapFilter); + EXPECT_THROW(parse_detector("both"), std::invalid_argument); +} + +TEST(PipelineCheck, TheDefaultSetupIsClean) +{ + PipelineSettings s; + s.filters = {kInflation}; + const auto r = check_pipeline(s); + EXPECT_TRUE(r.errors.empty()); + EXPECT_TRUE(r.warnings.empty()); + EXPECT_TRUE(r.sensors_processed); + EXPECT_TRUE(r.memory_effective); + EXPECT_EQ(r.planner_layer, "inflated_obstacles"); +} + +TEST(PipelineCheck, InflationIsIndependentOfTheDetector) +{ + for (const auto detector : {ObstacleDetector::BigmapDynamic, ObstacleDetector::NavmapFilter}) { + PipelineSettings s; + s.detector = detector; + s.memory_enabled = false; + if (detector == ObstacleDetector::NavmapFilter) {s.filters = {kObstacleFilter};} + EXPECT_EQ(check_pipeline(s).planner_layer, "obstacles"); // not listed: not inflated + s.filters.push_back(kInflation); + const auto r = check_pipeline(s); + EXPECT_EQ(r.planner_layer, "inflated_obstacles"); + EXPECT_TRUE(r.errors.empty()); + } +} + +TEST(PipelineCheck, TheFilterDetectorNeedsAnObstacleFilter) +{ + PipelineSettings s; + s.detector = ObstacleDetector::NavmapFilter; + s.memory_enabled = false; + s.filters = {kInflation}; + EXPECT_TRUE(mentions(check_pipeline(s).errors, "no obstacle filter")); + s.filters = {kObstacleFilter, kInflation}; + const auto r = check_pipeline(s); + EXPECT_TRUE(r.errors.empty()); + EXPECT_TRUE(r.warnings.empty()); + EXPECT_FALSE(r.sensors_processed); // the filter analyses them, not bigmap +} + +TEST(PipelineCheck, TwoDetectorsAtOnceAreAWarning) +{ + PipelineSettings s; + s.filters = {kObstacleFilter, kInflation}; + const auto r = check_pipeline(s); + EXPECT_TRUE(r.errors.empty()); + EXPECT_TRUE(mentions(r.warnings, "both write")); +} + +TEST(PipelineCheck, MemoryNeedsTheBigmapDetector) +{ + PipelineSettings s; + s.detector = ObstacleDetector::NavmapFilter; + s.memory_enabled = true; + s.filters = {kObstacleFilter, kInflation}; + const auto r = check_pipeline(s); + EXPECT_FALSE(r.memory_effective); + EXPECT_TRUE(mentions(r.warnings, "memory is off")); +} + +TEST(PipelineCheck, SensorsAreAnalysedForTheDynamicMapOrForTheMemory) +{ + PipelineSettings s; + s.filters = {kInflation}; + s.dynamic_map_enabled = false; + s.memory_enabled = false; + auto r = check_pipeline(s); + EXPECT_FALSE(r.sensors_processed); + EXPECT_TRUE(mentions(r.notes, "no sensor obstacles")); + s.dynamic_map_enabled = true; + EXPECT_TRUE(check_pipeline(s).sensors_processed); + s.dynamic_map_enabled = false; + s.memory_enabled = true; + EXPECT_TRUE(check_pipeline(s).sensors_processed); +} + +TEST(PipelineCheck, DetectionsNeedRemovalToReachTheObstaclesLayer) +{ + PipelineSettings s; + s.filters = {kInflation}; + s.removal_enabled = false; + EXPECT_TRUE(mentions(check_pipeline(s).warnings, "planner does not see it")); + s.memory_enabled = false; // nothing analysed: nothing to lose + EXPECT_TRUE(check_pipeline(s).warnings.empty()); +} + +TEST(PipelineCheck, TheInflationListedBeforeTheObstacleFilterIsAWarning) +{ + PipelineSettings s; + s.detector = ObstacleDetector::NavmapFilter; + s.memory_enabled = false; + s.filters = {kInflation, kObstacleFilter}; + EXPECT_TRUE(mentions(check_pipeline(s).warnings, "previous cycle")); + s.filters = {kObstacleFilter, kInflation}; + EXPECT_TRUE(check_pipeline(s).warnings.empty()); +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_real_maps.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_real_maps.cpp new file mode 100644 index 00000000..136517d0 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_real_maps.cpp @@ -0,0 +1,696 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Runs the surface LOD on a real NavMap file: BIGMAP_TEST_NAVMAP= (skipped without it) + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/lod_mesher.hpp" +#include "easynav_bigmap_maps_manager/narrow_passages.hpp" +#include "easynav_bigmap_maps_manager/navmap_dem.hpp" +#include "easynav_bigmap_maps_manager/surface_lod.hpp" +#include "navmap_core/DistanceField.hpp" +#include "navmap_core/Costs.hpp" +#include "navmap_ros/navmap_io.hpp" + +namespace +{ +// Triangles reachable through shared edges from the one nearest to (0, 0, 0) over cells whose slope +// is at most `max_deg`; returns the heights (rounded to metres) of what is reached +template +std::map flood( + const PosX & X, const PosY & Y, const PosZ & Z, + const std::vector> & tris, + double max_deg, double sx, double sy, double sz, double keep_off = 0.0) +{ + std::map, std::uint32_t> weld; + std::vector id(X.size()); + for (std::size_t v = 0; v < id.size(); ++v) { + const std::array k{std::lround(X[v] * 1000.0), std::lround(Y[v] * 1000.0), + std::lround(Z[v] * 1000.0)}; + id[v] = weld.emplace(k, static_cast(weld.size())).first->second; + } + const std::size_t nt = tris.size(); + std::vector ok(nt, 1); + for (std::size_t t = 0; t < nt; ++t) { + const auto a = tris[t][0], b = tris[t][1], d = tris[t][2]; + const double ux = X[b] - X[a], uy = Y[b] - Y[a], uz = Z[b] - Z[a]; + const double vx = X[d] - X[a], vy = Y[d] - Y[a], vz = Z[d] - Z[a]; + const double nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx; + const double len = std::sqrt(nx * nx + ny * ny + nz * nz); + ok[t] = len > 1e-12 && std::acos(std::min(1.0, std::abs(nz) / len)) * 180.0 / M_PI <= max_deg; + } + std::map, std::vector> edges; + for (std::size_t t = 0; t < nt; ++t) { + for (int k = 0; k < 3; ++k) { + auto a = id[tris[t][k]], b = id[tris[t][(k + 1) % 3]]; + if (a > b) {std::swap(a, b);} + edges[{a, b}].push_back(static_cast(t)); + } + } + if (keep_off > 0.0) { + // triangles whose centre is closer than keep_off to a border vertex (an edge of one triangle) + std::vector border; + for (const auto & e : edges) { + if (e.second.size() == 1) { + const auto t = e.second[0]; + for (int k = 0; k < 3; ++k) { + const auto a = id[tris[t][k]], b = id[tris[t][(k + 1) % 3]]; + if (std::make_pair(std::min(a, b), std::max(a, b)) == e.first) { + border.push_back(tris[t][k]); + border.push_back(tris[t][(k + 1) % 3]); + } + } + } + } + std::multimap, std::uint32_t> grid; + for (const auto v : border) { + grid.emplace(std::make_pair(std::lround(X[v] / 1.0), std::lround(Y[v] / 1.0)), v); + } + for (std::size_t t = 0; t < nt; ++t) { + const double cx = (X[tris[t][0]] + X[tris[t][1]] + X[tris[t][2]]) / 3.0; + const double cy = (Y[tris[t][0]] + Y[tris[t][1]] + Y[tris[t][2]]) / 3.0; + const double cz = (Z[tris[t][0]] + Z[tris[t][1]] + Z[tris[t][2]]) / 3.0; + const std::int64_t gx = std::lround(cx), gy = std::lround(cy); + for (std::int64_t j = gy - 2; j <= gy + 2 && ok[t]; ++j) { + for (std::int64_t i = gx - 2; i <= gx + 2 && ok[t]; ++i) { + const auto range = grid.equal_range({i, j}); + for (auto it = range.first; it != range.second; ++it) { + const auto v = it->second; + if (std::abs(Z[v] - cz) < 0.8 && std::hypot(X[v] - cx, Y[v] - cy) < keep_off) { + ok[t] = 0; + break; + } + } + } + } + } + } + std::size_t start = 0; + double best = 1e18; + for (std::size_t t = 0; t < nt; ++t) { + const auto v = tris[t][0]; + const double d = std::hypot(X[v] - sx, Y[v] - sy) + 3.0 * std::abs(Z[v] - sz); + if (ok[t] && d < best) {best = d; start = t;} + } + std::vector seen(nt, 0); + std::vector st{start}; + seen[start] = 1; + std::map heights; + while (!st.empty()) { + const auto t = st.back(); + st.pop_back(); + ++heights[static_cast(std::lround(Z[tris[t][0]]))]; + for (int k = 0; k < 3; ++k) { + auto a = id[tris[t][k]], b = id[tris[t][(k + 1) % 3]]; + if (a > b) {std::swap(a, b);} + for (const auto n : edges[{a, b}]) { + if (!seen[n] && ok[n]) {seen[n] = 1; st.push_back(n);} + } + } + } + return heights; +} + +void print_heights(const char * what, const std::map & h) +{ + std::printf("[flood] %s reaches z:", what); + for (const auto & kv : h) { + std::printf(" %d(%zu)", kv.first, kv.second); + } + std::printf("\n"); +} +} // namespace + +namespace +{ +// What the planner can drive: cells farther than `inscribed` (the robot's radius) from a border +// (the way the InflationFilter measures it), reached through shared edges from the one nearest to +// (0, 0, 0), over cells no steeper than `max_deg` +std::map drivable( + ::navmap::NavMap & nm, double inscribed, double max_deg, + const std::vector * slope = nullptr) +{ + nm.surfaces.resize(1); + nm.surfaces[0].navcels.resize(nm.navcels.size()); + for (std::size_t t = 0; t < nm.navcels.size(); ++t) { + nm.surfaces[0].navcels[t] = static_cast<::navmap::NavCelId>(t); + } + nm.rebuild_geometry_accels(); + ::navmap::DistanceField df(nm, true, 3.0f); + df.compute({}); + const std::size_t n = nm.navcels.size(); + std::vector ok(n, 0); + for (std::size_t t = 0; t < n; ++t) { + double deg = 0.0; + if (slope) { + deg = (*slope)[t]; + } else { + deg = std::acos(std::min(1.0, std::abs(static_cast(nm.navcels[t].normal.z())))) * + 180.0 / M_PI; + } + ok[t] = df[static_cast<::navmap::NavCelId>(t)] > inscribed && deg <= max_deg; + } + std::size_t start = n; + double best = 1e18; + for (std::size_t t = 0; t < n; ++t) { + const auto v = nm.navcels[t].v[0]; + const double d = std::hypot(nm.positions.x[v], nm.positions.y[v]) + + 3.0 * std::abs(nm.positions.z[v]); + if (ok[t] && d < best) {best = d; start = t;} + } + std::map heights; + if (start == n) {return heights;} + std::vector seen(n, 0); + std::vector st{start}; + seen[start] = 1; + while (!st.empty()) { + const auto t = st.back(); + st.pop_back(); + ++heights[static_cast(std::lround(nm.positions.z[nm.navcels[t].v[0]]))]; + for (const auto nb : nm.navcel_neighbors(static_cast<::navmap::NavCelId>(t))) { + if (!seen[nb] && ok[nb]) {seen[nb] = 1; st.push_back(nb);} + } + } + return heights; +} +} // namespace + +TEST(RealMap, SurfaceLod) +{ + const char * path = std::getenv("BIGMAP_TEST_NAVMAP"); + if (!path) {GTEST_SKIP() << "BIGMAP_TEST_NAVMAP not set";} + ::navmap::NavMap nm; + std::error_code ec; + ASSERT_TRUE(navmap_ros::io::load_from_file(path, nm, &ec)) << ec.message(); + std::size_t up = 0, down = 0; + float zmin = 1e9f, zmax = -1e9f, xmin = 1e9f, xmax = -1e9f, ymin = 1e9f, ymax = -1e9f; + for (const auto & c : nm.navcels) { + const auto a = c.v[0], b = c.v[1], d = c.v[2]; + const double nz = (nm.positions.x[b] - nm.positions.x[a]) * + (nm.positions.y[d] - nm.positions.y[a]) - + (nm.positions.y[b] - nm.positions.y[a]) * (nm.positions.x[d] - nm.positions.x[a]); + (nz > 0 ? up : down)++; + } + for (std::size_t v = 0; v < nm.positions.x.size(); ++v) { + zmin = std::min(zmin, nm.positions.z[v]); zmax = std::max(zmax, nm.positions.z[v]); + xmin = std::min(xmin, nm.positions.x[v]); xmax = std::max(xmax, nm.positions.x[v]); + ymin = std::min(ymin, nm.positions.y[v]); ymax = std::max(ymax, nm.positions.y[v]); + } + std::printf( + "[map] %zu vertices, %zu NavCels (%zu facing up, %zu down), x %.1f..%.1f y %.1f..%.1f " + "z %.2f..%.2f, colours %s, layers:", nm.positions.x.size(), nm.navcels.size(), up, down, + xmin, xmax, ymin, ymax, zmin, zmax, nm.colors ? "yes" : "no"); + for (const auto & l : nm.list_layers()) { + std::printf(" %s", l.c_str()); + } + std::printf("\n"); + + std::size_t slope_bins[7] = {0}; // <5, <10, <15, <20, <25, <30, >=30 degrees + for (const auto & c : nm.navcels) { + const auto a = c.v[0], b = c.v[1], d = c.v[2]; + const auto & P = nm.positions; + const double ux = P.x[b] - P.x[a], uy = P.y[b] - P.y[a], uz = P.z[b] - P.z[a]; + const double vx = P.x[d] - P.x[a], vy = P.y[d] - P.y[a], vz = P.z[d] - P.z[a]; + const double nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx; + const double len = std::sqrt(nx * nx + ny * ny + nz * nz); + const double deg = std::acos(std::min(1.0, std::abs(nz) / len)) * 180.0 / M_PI; + ++slope_bins[std::min(6, static_cast(deg / 5.0))]; + } + std::printf("[slope] NavCels by slope <5/<10/<15/<20/<25/<30/>=30 deg:"); + for (const auto n : slope_bins) { + std::printf(" %zu", n); + } + std::printf("\n"); + + if (std::getenv("BIGMAP_DUMP_LEVELS")) { + // heights of the surfaces over a 3 m grid (rows: y from the top) + for (float y = ymax; y >= ymin; y -= 3.0f) { + for (float x = xmin; x <= xmax; x += 3.0f) { + std::set zs; + for (std::size_t v = 0; v < nm.positions.x.size(); ++v) { + if (std::abs(nm.positions.x[v] - x) < 1.5f && std::abs(nm.positions.y[v] - y) < 1.5f) { + zs.insert(static_cast(std::lround(nm.positions.z[v]))); + } + } + std::printf("%-8s", [&zs]() { + std::string t; + for (const auto z : zs) { + t += std::to_string(z) + ","; + } + return t.empty() ? std::string(".") : t; + }().c_str()); + } + std::printf(" y=%.0f\n", y); + } + } + + { + // connectivity of the source mesh itself (shared edges, vertices welded to a millimetre) + std::map, std::uint32_t> weld; + std::vector id(nm.positions.x.size()); + for (std::size_t v = 0; v < id.size(); ++v) { + const std::array k{std::lround(nm.positions.x[v] * 1000.0), + std::lround(nm.positions.y[v] * 1000.0), std::lround(nm.positions.z[v] * 1000.0)}; + id[v] = weld.emplace(k, static_cast(weld.size())).first->second; + } + const std::size_t nt = nm.navcels.size(); + std::map, std::vector> edges; + for (std::size_t t = 0; t < nt; ++t) { + for (int k = 0; k < 3; ++k) { + auto a = id[nm.navcels[t].v[k]], b = id[nm.navcels[t].v[(k + 1) % 3]]; + if (a > b) {std::swap(a, b);} + edges[{a, b}].push_back(static_cast(t)); + } + } + std::vector comp(nt, -1); + std::vector csize; + std::vector> cz; + for (std::size_t t0 = 0; t0 < nt; ++t0) { + if (comp[t0] >= 0) {continue;} + const int cid = static_cast(csize.size()); + csize.push_back(0); + cz.emplace_back(); + std::vector st{t0}; + comp[t0] = cid; + while (!st.empty()) { + const auto t = st.back(); + st.pop_back(); + ++csize[cid]; + cz[cid].insert(static_cast(std::lround(nm.positions.z[nm.navcels[t].v[0]]))); + for (int k = 0; k < 3; ++k) { + auto a = id[nm.navcels[t].v[k]], b = id[nm.navcels[t].v[(k + 1) % 3]]; + if (a > b) {std::swap(a, b);} + for (const auto n : edges[{a, b}]) { + if (comp[n] < 0) {comp[n] = cid; st.push_back(n);} + } + } + } + } + std::printf("[source topology] %zu components; the 6 largest:", csize.size()); + std::vector order(csize.size()); + for (std::size_t i = 0; i < order.size(); ++i) { + order[i] = i; + } + std::sort(order.begin(), order.end(), [&](auto a, auto b) {return csize[a] > csize[b];}); + for (std::size_t i = 0; i < std::min(6, order.size()); ++i) { + std::printf(" %zu(z:", csize[order[i]]); + for (const auto z : cz[order[i]]) { + std::printf("%d,", z); + } + std::printf(")"); + } + std::printf("\n"); + // the component under the origin at floor 0 + int best = -1; + double bd = 1e9; + for (std::size_t t = 0; t < nt; ++t) { + const auto v = nm.navcels[t].v[0]; + const double d = std::hypot(nm.positions.x[v], nm.positions.y[v]) + + 3.0 * std::abs(nm.positions.z[v]); + if (d < bd) {bd = d; best = comp[t];} + } + std::printf( + "[source topology] the component at (0, 0, 0): %zu NavCels, floors z:", csize[best]); + for (const auto z : cz[best]) { + std::printf(" %d", z); + } + std::printf("\n"); + } + + { + std::vector> tris; + for (const auto & c : nm.navcels) { + tris.push_back({c.v[0], c.v[1], c.v[2]}); + } + for (const double off : {0.0, 0.5, 0.7}) { + char name[96]; + std::snprintf(name, sizeof(name), "source, slope <= 30 deg, %.1f m off the borders", off); + print_heights( + name, flood(nm.positions.x, nm.positions.y, nm.positions.z, tris, 30.0, 0, 0, 0, off)); + } + } + + { + auto copy = nm; + print_heights("source, drivable by a 0.7 m robot", drivable(copy, 0.7, 30.0)); + } + + using clock = std::chrono::steady_clock; + auto t0 = clock::now(); + bigmap_maps_manager::SurfaceLodMesher::Config cfg; + if (const char * ms = std::getenv("BIGMAP_MIN_SIZE")) {cfg.min_size = std::atof(ms);} + if (const char * kn = std::getenv("BIGMAP_NARROW")) {cfg.keep_narrow_width = std::atof(kn);} + bigmap_maps_manager::SurfaceLodMesher mesher(nm, cfg); + std::printf( + "[build] %.1f ms: %zu welded vertices, %zu triangles, %d levels\n", + std::chrono::duration(clock::now() - t0).count(), mesher.vertex_count(), + mesher.source_triangle_count(), mesher.level_count()); + bigmap_maps_manager::LodParams p; + p.high_res_radius = 10; p.low_res_distance = 30; p.min_size = cfg.min_size; p.max_size = 32; + if (const char * hr = std::getenv("BIGMAP_HIGH_RES")) {p.high_res_radius = std::atof(hr);} + p.known_margin = 1; p.known_falloff = 5; + const float xs[] = {0.5f * (xmin + xmax), xmin, xmax}; + for (const float x : xs) { + t0 = clock::now(); + const auto m = mesher.extract(x, 0.5f * (ymin + ymax), zmin, p); + std::printf( + "[extract] robot x=%.1f: %zu triangles, %zu vertices, %.1f ms\n", x, m.triangle_count(), + m.x.size(), std::chrono::duration(clock::now() - t0).count()); + { + // what BigMapMapsManager::build_navmap does with the mesh + t0 = clock::now(); + ::navmap::NavMap out; + out.positions.x = m.x; out.positions.y = m.y; out.positions.z = m.z; + out.navcels.resize(m.triangle_count()); + out.surfaces.resize(1); + out.surfaces[0].navcels.resize(m.triangle_count()); + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + for (int k = 0; k < 3; ++k) { + out.navcels[t].v[k] = m.tri[3 * t + k]; + } + out.surfaces[0].navcels[t] = static_cast<::navmap::NavCelId>(t); + } + const auto t1 = clock::now(); + out.rebuild_geometry_accels(); + { + const auto t2 = clock::now(); + out.add_layer("occupancy", "", "", ::navmap::FREE_SPACE); + out.add_layer("obstacles", "", "", ::navmap::FREE_SPACE); + out.add_layer("elevation", "", "m"); + out.add_layer("lod_size", "", "m"); + out.add_layer("traversed", "", "", 0); + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto id = static_cast<::navmap::NavCelId>(t); + out.layer_set("elevation", id, 1.0f); + out.layer_set("lod_size", id, m.leg[t]); + if (t % 7 == 0) {out.layer_set("traversed", id, 255);} + } + std::printf( + " layers (add + set per NavCel): %.1f ms\n", + std::chrono::duration(clock::now() - t2).count()); + } + std::printf( + " rebuild_geometry_accels: %.1f ms\n", + std::chrono::duration(clock::now() - t1).count()); + (void)t0; + } + { + // topology: components through shared edges, and edges shared by more than two triangles + const std::size_t nt = m.triangle_count(); + std::map, std::vector> edges; + for (std::size_t t = 0; t < nt; ++t) { + for (int k = 0; k < 3; ++k) { + auto a = m.tri[3 * t + k], b = m.tri[3 * t + (k + 1) % 3]; + if (a > b) {std::swap(a, b);} + edges[{a, b}].push_back(static_cast(t)); + } + } + std::size_t nonmanifold = 0, border = 0; + for (const auto & e : edges) { + nonmanifold += e.second.size() > 2; + border += e.second.size() == 1; + } + std::vector comp(nt, -1); + std::vector sizes_c; + for (std::size_t t0 = 0; t0 < nt; ++t0) { + if (comp[t0] >= 0) {continue;} + const int id = static_cast(sizes_c.size()); + sizes_c.push_back(0); + std::vector st{t0}; + comp[t0] = id; + while (!st.empty()) { + const auto t = st.back(); + st.pop_back(); + ++sizes_c[id]; + for (int k = 0; k < 3; ++k) { + auto a = m.tri[3 * t + k], b = m.tri[3 * t + (k + 1) % 3]; + if (a > b) {std::swap(a, b);} + for (const auto n : edges[{a, b}]) { + if (comp[n] < 0) {comp[n] = id; st.push_back(n);} + } + } + } + } + std::sort(sizes_c.rbegin(), sizes_c.rend()); + std::printf( + " topology: %zu components (largest %zu, 2nd %zu), %zu non-manifold edges, " + "%zu border edges\n", sizes_c.size(), sizes_c[0], sizes_c.size() > 1 ? sizes_c[1] : 0, + nonmanifold, border); + } + { + std::vector> tris; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + tris.push_back({m.tri[3 * t], m.tri[3 * t + 1], m.tri[3 * t + 2]}); + } + for (const double off : {0.0, 0.5, 0.7}) { + char name[96]; + std::snprintf( + name, sizeof(name), "LOD mesh (robot x=%.1f), slope <= 30, %.1f m off borders", x, off); + print_heights(name, flood(m.x, m.y, m.z, tris, 90.0, 0, 0, 0, off)); + } + } + { + std::size_t bins[8] = {0}, flipped = 0; // slope <10/20/30/40/50/60/>=60 degrees + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], d = m.tri[3 * t + 2]; + const double ux = m.x[b] - m.x[a], uy = m.y[b] - m.y[a], uz = m.z[b] - m.z[a]; + const double vx = m.x[d] - m.x[a], vy = m.y[d] - m.y[a], vz = m.z[d] - m.z[a]; + const double nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx; + const double len = std::sqrt(nx * nx + ny * ny + nz * nz); + if (len < 1e-12) {++bins[7]; continue;} + flipped += nz < 0; + ++bins[std::min( + 6, static_cast( + std::acos(std::min(1.0, std::abs(nz) / len)) * 18.0 / M_PI))]; + } + std::printf(" slope <10/20/30/40/50/60/>=60/degenerate:"); + for (int k = 0; k < 8; ++k) { + std::printf(" %zu", bins[k]); + } + std::printf("; facing down: %zu\n", flipped); + } + { + ::navmap::NavMap out; + out.positions.x = m.x; out.positions.y = m.y; out.positions.z = m.z; + out.navcels.resize(m.triangle_count()); + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + for (int k = 0; k < 3; ++k) { + out.navcels[t].v[k] = m.tri[3 * t + k]; + } + } + for (const double ins : {0.0, 0.3, 0.5, 0.7}) { + char name[96]; + std::snprintf(name, sizeof(name), "LOD mesh, %.1f m from the borders", ins); + print_heights(name, drivable(out, ins, 90.0, &m.slope)); + } + std::printf(" non-manifold edges (open for the planner): %zu\n", + out.nonmanifold_edges()); + } + std::size_t sizes[8] = {0}; + for (const auto l : m.leg) { + std::size_t k = 0; + for (float s = 1.0f; s < l - 0.01f && k < 7; s *= 2.0f) { + ++k; + } + ++sizes[k]; + } + std::printf(" leg 1/2/4/8/16/32/64:"); + for (int k = 0; k < 7; ++k) { + std::printf(" %zu", sizes[k]); + } + std::printf("\n"); + } +} + +// --------------------------------------------------------------------------------------------- +// BIGMAP_TEST_NAVMAP= BIGMAP_COMPARE=1: the two engines on the same high-resolution map, +// against +// the map itself: height error and obstacles (slope > 25 deg) by distance from the robot. +// grid : the map rasterised to a 1 m height grid, RTIN (what a DEM source or a plain +// initial_navmap gets); a NavCel is an obstacle if its own plane is steeper +// hierarchy : SurfaceLodMesher on the NavMap's own triangles; an obstacle if the steepest source +// NavCel it stands for is steeper +namespace +{ +struct Raster +{ + int n; double res; + std::vector z; std::vector lethal; // lethal: -1 uncovered +}; + +Raster rasterise( + const bigmap_maps_manager::LodMesh & m, const std::vector & leth, int n, + double res) +{ + Raster r{n, res, std::vector(static_cast(n) * n, -1e9f), + std::vector(static_cast(n) * n, -1)}; + const double c = (n - 1) / 2.0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], d = m.tri[3 * t + 2]; + const double x0 = m.x[a], y0 = m.y[a], x1 = m.x[b], y1 = m.y[b], x2 = m.x[d], y2 = m.y[d]; + const double det = (y1 - y2) * (x0 - x2) + (x2 - x1) * (y0 - y2); + if (std::abs(det) < 1e-9) {continue;} + const int i0 = std::max(0, + static_cast(std::ceil(std::min({x0, x1, x2}) / res + c - 1e-9))); + const int i1 = std::min(n - 1, + static_cast(std::floor(std::max({x0, x1, x2}) / res + c + 1e-9))); + const int j0 = std::max(0, + static_cast(std::ceil(std::min({y0, y1, y2}) / res + c - 1e-9))); + const int j1 = std::min(n - 1, + static_cast(std::floor(std::max({y0, y1, y2}) / res + c + 1e-9))); + for (int j = j0; j <= j1; ++j) { + for (int i = i0; i <= i1; ++i) { + const double px = (i - c) * res, py = (j - c) * res; + const double l0 = ((y1 - y2) * (px - x2) + (x2 - x1) * (py - y2)) / det; + const double l1 = ((y2 - y0) * (px - x2) + (x0 - x2) * (py - y2)) / det; + const double l2 = 1.0 - l0 - l1; + if (l0 < -1e-9 || l1 < -1e-9 || l2 < -1e-9) {continue;} + const float z = static_cast(l0 * m.z[a] + l1 * m.z[b] + l2 * m.z[d]); + const std::size_t k = static_cast(j) * n + i; + if (z > r.z[k]) {r.z[k] = z; r.lethal[k] = leth[t] ? 1 : 0;} + } + } + } + return r; +} +} // namespace + +TEST(RealMap, EngineComparison) +{ + const char * path = std::getenv("BIGMAP_TEST_NAVMAP"); + if (!path || !std::getenv("BIGMAP_COMPARE")) { + GTEST_SKIP() << "BIGMAP_TEST_NAVMAP + BIGMAP_COMPARE"; + } + ::navmap::NavMap nm; + std::error_code ec; + ASSERT_TRUE(navmap_ros::io::load_from_file(path, nm, &ec)) << ec.message(); + const double max_slope = 25.0, hi_res = 0.5; + // the map itself, as a 0.5 m grid: the reference + const auto src = bigmap_maps_manager::dem_from_navmap(nm, hi_res); + const int n = src.dem.side; + const double c = (n - 1) / 2.0; + std::vector src_leth(static_cast(n) * n, 0); + for (int j = 0; j + 1 < n; ++j) { + for (int i = 0; i + 1 < n; ++i) { + const double zx = (src.dem.at(i + 1, j) - src.dem.at(i, j)) / hi_res; + const double zy = (src.dem.at(i, j + 1) - src.dem.at(i, j)) / hi_res; + src_leth[static_cast(j) * n + i] = + std::atan(std::hypot(zx, zy)) * 180.0 / M_PI > max_slope; + } + } + bigmap_maps_manager::LodParams p; + p.high_res_radius = 20; p.low_res_distance = 80; p.min_size = 1.0; p.max_size = 32; + p.base_error = 0.02; p.error_growth = 0.05; p.known_margin = 1; p.known_falloff = 5; + const double rx = 0.0, ry = 0.0; + const double rz = src.dem.height_at(rx, ry); + + using clock = std::chrono::steady_clock; + struct Result {const char * name; Raster r; std::size_t tris; double build_ms, extract_ms;}; + std::vector results; + { // grid engine: what the manager does with a plain initial_navmap (1 m grid, RTIN) + auto t0 = clock::now(); + auto nd = bigmap_maps_manager::dem_from_navmap(nm, p.min_size); + double narrow = 0.0; + if (const char * kn = std::getenv("BIGMAP_NARROW")) {narrow = std::atof(kn);} + const auto pinned = bigmap_maps_manager::narrow_passage_nodes(nd.dem, nd.valid, max_slope, + narrow); + std::printf("grid engine: %zu of %zu nodes narrow (width %.1f)\n", + static_cast(std::count(pinned.begin(), pinned.end(), 1)), nd.dem.z.size(), + narrow); + bigmap_maps_manager::LodMesher mesher(nd.dem, nd.valid, pinned); + const double build = std::chrono::duration(clock::now() - t0).count(); + t0 = clock::now(); + const auto m = mesher.extract(rx, ry, p, nullptr); + const double ex = std::chrono::duration(clock::now() - t0).count(); + const float min_nz = static_cast(std::cos(max_slope * M_PI / 180.0)); + std::vector leth(m.triangle_count()); + for (std::size_t t = 0; t < leth.size(); ++t) { + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], d = m.tri[3 * t + 2]; + const double ux = m.x[b] - m.x[a], uy = m.y[b] - m.y[a], uz = m.z[b] - m.z[a]; + const double vx = m.x[d] - m.x[a], vy = m.y[d] - m.y[a], vz = m.z[d] - m.z[a]; + const double nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx; + leth[t] = nz / std::sqrt(nx * nx + ny * ny + nz * nz) < min_nz; + } + results.push_back({"grid (RTIN, 1 m DEM)", rasterise(m, leth, n, hi_res), m.triangle_count(), + build, ex}); + } + { + auto t0 = clock::now(); + bigmap_maps_manager::SurfaceLodMesher::Config cfg; + cfg.min_size = 1.0; cfg.max_size = 32.0; + bigmap_maps_manager::SurfaceLodMesher mesher(nm, cfg); + const double build = std::chrono::duration(clock::now() - t0).count(); + t0 = clock::now(); + const auto m = mesher.extract(rx, ry, rz, p); + const double ex = std::chrono::duration(clock::now() - t0).count(); + std::vector leth(m.triangle_count()); + for (std::size_t t = 0; t < leth.size(); ++t) { + leth[t] = m.slope[t] > max_slope; + } + results.push_back({"hierarchy (NavMap triangles)", rasterise(m, leth, n, hi_res), + m.triangle_count(), build, ex}); + } + const double bands[] = {0, 20, 50, 100, 1e9}; + for (const auto & res : results) { + std::printf("[compare] %s: %zu triangles, build %.0f ms, extract %.1f ms\n", res.name, res.tris, + res.build_ms, res.extract_ms); + for (int bnd = 0; bnd < 4; ++bnd) { + std::vector err; + std::size_t cells = 0, holes = 0, s_l = 0, both = 0, l_only = 0, l_total = 0, free_cells = 0; + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + const double d = std::hypot((i - c) * hi_res - rx, (j - c) * hi_res - ry); + if (d < bands[bnd] || d >= bands[bnd + 1]) {continue;} + const std::size_t k = static_cast(j) * n + i; + if (!src.valid[k]) {continue;} + ++cells; + if (res.r.lethal[k] < 0) {++holes; continue;} + err.push_back(std::abs(res.r.z[k] - src.dem.z[k])); + const bool sl = src_leth[k], ll = res.r.lethal[k] == 1; + s_l += sl; l_total += ll; + if (sl && ll) {++both;} + if (!sl) {++free_cells; if (ll) {++l_only;}} + } + } + std::sort(err.begin(), err.end()); + auto pc = [&](double q) { + return err.empty() ? 0.0f : err[static_cast(q * (err.size() - 1))]; + }; + std::printf( + " %3.0f-%3.0f m: %7zu cells, holes %.2f%%, |dz| median %.2f p95 %.2f max %.2f m; " + "obstacles: source %.1f%% of cells, kept %.1f%%, false %.1f%% of the free ones\n", + bands[bnd], std::min(bands[bnd + 1], 400.0), cells, + 100.0 * holes / std::max(cells, 1), + pc(0.5), pc(0.95), err.empty() ? 0.0f : err.back(), + 100.0 * s_l / std::max(cells - holes, 1), + 100.0 * both / std::max(s_l, 1), + 100.0 * l_only / std::max(free_cells, 1)); + } + } +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_surface_lod.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_surface_lod.cpp new file mode 100644 index 00000000..dff6f11f --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_surface_lod.cpp @@ -0,0 +1,644 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/surface_lod.hpp" +#include "navmap_core/NavMap.hpp" + +using bigmap_maps_manager::LodMesh; +using bigmap_maps_manager::LodParams; +using bigmap_maps_manager::SurfaceLodMesher; + +namespace +{ +// A rectangular patch of triangles of leg h over [x0, x1] x [y0, y1] with height z(x, y). Its +// vertices are its own: neighbouring patches meet only through the welding of equal positions. +void add_patch( + ::navmap::NavMap & nm, double x0, double x1, double y0, double y1, double h, + const std::function & z, bool mark_traversed = false) +{ + const int nx = static_cast(std::lround((x1 - x0) / h)); + const int ny = static_cast(std::lround((y1 - y0) / h)); + const auto base = static_cast(nm.positions.x.size()); + for (int j = 0; j <= ny; ++j) { + for (int i = 0; i <= nx; ++i) { + const double x = x0 + i * h, y = y0 + j * h; + nm.positions.x.push_back(static_cast(x)); + nm.positions.y.push_back(static_cast(y)); + nm.positions.z.push_back(static_cast(z(x, y))); + } + } + auto vid = [&](int i, int j) {return base + static_cast(j * (nx + 1) + i);}; + for (int j = 0; j < ny; ++j) { + for (int i = 0; i < nx; ++i) { + for (int half = 0; half < 2; ++half) { + ::navmap::NavCel c; + if (half == 0) { + c.v[0] = vid(i, j); c.v[1] = vid(i + 1, j); c.v[2] = vid(i + 1, j + 1); + } else { + c.v[0] = vid(i, j); c.v[1] = vid(i + 1, j + 1); c.v[2] = vid(i, j + 1); + } + nm.navcels.push_back(c); + (void)mark_traversed; + } + } + } +} + +// Two floors one over the other (z = 0 and z = 3) joined by a ramp that starts over the lower one: +// floor 0: x in [-30, 20] ramp: x in [-20, -10], z from 0 to 3 floor 1: x in [-10, 20] +::navmap::NavMap garage() +{ + ::navmap::NavMap nm; + add_patch(nm, -30, 20, -10, 10, 0.5, [](double, double) {return 0.0;}); + add_patch(nm, -20, -10, -10, 10, 0.5, [](double x, double) {return 0.3 * (x + 20.0);}); + add_patch(nm, -10, 20, -10, 10, 0.5, [](double, double) {return 3.0;}); + return nm; +} + +LodParams params() +{ + LodParams p; + p.high_res_radius = 10.0; + p.low_res_distance = 30.0; + p.min_size = 1.0; + p.max_size = 16.0; + p.base_error = 0.02; + p.error_growth = 0.05; + p.known_margin = 1.0; + p.known_falloff = 4.0; + return p; +} + +SurfaceLodMesher::Config config() +{ + SurfaceLodMesher::Config c; + c.min_size = 1.0; + c.max_size = 16.0; + c.cluster_height = 1.2; + c.vertical_tolerance = 1.5; + return c; +} + +struct Centroid +{ + double x, y, z; +}; + +Centroid centroid(const LodMesh & m, std::size_t t) +{ + const auto a = m.tri[3 * t], b = m.tri[3 * t + 1], c = m.tri[3 * t + 2]; + return {(m.x[a] + m.x[b] + m.x[c]) / 3.0, (m.y[a] + m.y[b] + m.y[c]) / 3.0, + (m.z[a] + m.z[b] + m.z[c]) / 3.0}; +} + +// Can a walker go from a triangle for which `from` holds to one for which `to` holds crossing +// shared edges only? +bool connected( + const LodMesh & m, const std::function & from, + const std::function & to) +{ + const std::size_t nt = m.triangle_count(); + std::map, std::vector> edges; + for (std::size_t t = 0; t < nt; ++t) { + for (int k = 0; k < 3; ++k) { + auto a = m.tri[3 * t + k], b = m.tri[3 * t + (k + 1) % 3]; + if (a > b) {std::swap(a, b);} + edges[{a, b}].push_back(t); + } + } + std::vector seen(nt, 0); + std::queue q; + for (std::size_t t = 0; t < nt; ++t) { + if (from(centroid(m, t))) {seen[t] = 1; q.push(t);} + } + if (q.empty()) {return false;} + while (!q.empty()) { + const auto t = q.front(); + q.pop(); + if (to(centroid(m, t))) {return true;} + for (int k = 0; k < 3; ++k) { + auto a = m.tri[3 * t + k], b = m.tri[3 * t + (k + 1) % 3]; + if (a > b) {std::swap(a, b);} + for (const auto n : edges[{a, b}]) { + if (!seen[n]) {seen[n] = 1; q.push(n);} + } + } + } + return false; +} +} // namespace + +TEST(SurfaceLod, BuildsAHierarchy) +{ + SurfaceLodMesher mesher(garage(), config()); + EXPECT_GT(mesher.level_count(), 3); + EXPECT_GT(mesher.vertex_count(), 1000u); + // the three patches share their edges: welded vertices are fewer than the sum of the patches + EXPECT_LT(mesher.vertex_count(), 101u * 41u + 21u * 41u + 61u * 41u); +} + +TEST(SurfaceLod, FarAwayIsCoarserAndNearIsFine) +{ + SurfaceLodMesher mesher(garage(), config()); + const auto near_mesh = mesher.extract(15.0, 0.0, 0.0, params()); + EXPECT_LT(near_mesh.triangle_count(), mesher.source_triangle_count() / 2); + for (std::size_t t = 0; t < near_mesh.triangle_count(); ++t) { + const auto c = centroid(near_mesh, t); + if (std::hypot(c.x - 15.0, c.y) < 4.0 && std::abs(c.z) < 0.5) { + ASSERT_LE(near_mesh.leg[t], 1.01f) << "at " << c.x << ", " << c.y; + } + } + float far_leg = 0; + for (std::size_t t = 0; t < near_mesh.triangle_count(); ++t) { + const auto c = centroid(near_mesh, t); + if (c.x < -20.0) {far_leg = std::max(far_leg, near_mesh.leg[t]);} + } + EXPECT_GE(far_leg, 8.0f); +} + +TEST(SurfaceLod, FloorsAreNeverMerged) +{ + SurfaceLodMesher mesher(garage(), config()); + for (const auto & pos : {std::array{15, 0, 0}, std::array{-25, 0, 0}, + std::array{15, 0, 3}}) + { + const auto m = mesher.extract(pos[0], pos[1], pos[2], params()); + for (std::size_t v = 0; v < m.x.size(); ++v) { + if (m.x[v] > -9.0) { + ASSERT_TRUE(std::abs(m.z[v]) < 0.01 || std::abs(m.z[v] - 3.0) < 0.01) + << "x " << m.x[v] << " z " << m.z[v]; + } + } + } +} + +TEST(SurfaceLod, FloorsStayConnectedThroughTheRamp) +{ + SurfaceLodMesher mesher(garage(), config()); + const auto on_floor0 = [](const Centroid & c) {return c.x > 10.0 && std::abs(c.z) < 0.1;}; + const auto on_floor1 = [](const Centroid & c) {return c.x > 10.0 && std::abs(c.z - 3.0) < 0.1;}; + // wherever the robot is, the planner can still go up the ramp and the other way round + for (const auto & pos : {std::array{15, 0, 0}, std::array{15, 0, 3}, + std::array{-25, 0, 0}, std::array{-15, 0, 1.5}, + std::array{0, 5, 3}, std::array{-100, 0, 0}}) + { + const auto m = mesher.extract(pos[0], pos[1], pos[2], params()); + ASSERT_TRUE(connected(m, on_floor0, on_floor1)) + << "robot at " << pos[0] << ", " << pos[1] << ", " << pos[2]; + ASSERT_TRUE(connected(m, on_floor1, on_floor0)); + } +} + +TEST(SurfaceLod, NoFloorMeshWithoutARamp) +{ + // Without the ramp the floors are two components: the test above would not be meaningful if + // everything were connected anyway + ::navmap::NavMap nm; + add_patch(nm, -30, 20, -10, 10, 0.5, [](double, double) {return 0.0;}); + add_patch(nm, -10, 20, -10, 10, 0.5, [](double, double) {return 3.0;}); + SurfaceLodMesher mesher(nm, config()); + const auto m = mesher.extract(15.0, 0.0, 0.0, params()); + EXPECT_FALSE(connected( + m, [](const Centroid & c) {return std::abs(c.z) < 0.1;}, + [](const Centroid & c) {return std::abs(c.z - 3.0) < 0.1;})); +} + +TEST(SurfaceLod, DrivenOverGroundStaysFineOnItsOwnFloorOnly) +{ + SurfaceLodMesher mesher(garage(), config()); + EXPECT_GT(mesher.mark(15.0, 0.0, 0.0, 1.5), 0); + EXPECT_EQ(mesher.mark(15.0, 0.0, 0.0, 1.5), 0); // nothing new the second time + const auto m = mesher.extract(-25.0, 0.0, 0.0, params()); // the robot is 40 m away + bool fine_below = false, upper_is_coarse = true, flagged = false, upper_flagged = false; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto c = centroid(m, t); + if (std::hypot(c.x - 15.0, c.y) < 1.0) { + if (std::abs(c.z) < 0.1) { + fine_below = fine_below || m.leg[t] <= 1.01f; + flagged = flagged || m.traversed[t]; + } else if (std::abs(c.z - 3.0) < 0.1) { + upper_is_coarse = upper_is_coarse && m.leg[t] >= 4.0f; + upper_flagged = upper_flagged || m.traversed[t]; + } + } + } + EXPECT_TRUE(fine_below); + EXPECT_TRUE(flagged); + EXPECT_TRUE(upper_is_coarse); // the floor above was not driven over + EXPECT_FALSE(upper_flagged); + + // forgotten beyond a radius: the robot at the other end, 40 m away + EXPECT_GT(mesher.forget_outside(-25.0, 0.0, 0.0, 20.0), 0u); + EXPECT_EQ(mesher.traversed_count(), 0u); + const auto m2 = mesher.extract(-25.0, 0.0, 0.0, params()); + for (std::size_t t = 0; t < m2.triangle_count(); ++t) { + const auto c = centroid(m2, t); + if (std::hypot(c.x - 15.0, c.y) < 1.0) {ASSERT_GE(m2.leg[t], 4.0f);} + ASSERT_FALSE(m2.traversed[t]); + } +} + +TEST(SurfaceLod, RestoresTheTraversedLayerOfTheMap) +{ + auto nm = garage(); + nm.add_layer("traversed", "", "", 0); + for (std::size_t t = 0; t < nm.navcels.size(); ++t) { + const auto & v = nm.navcels[t].v; + if (nm.positions.x[v[0]] > 12.0 && nm.positions.z[v[0]] < 0.1) { + nm.layer_set("traversed", static_cast<::navmap::NavCelId>(t), 255); + } + } + SurfaceLodMesher mesher(nm, config()); + EXPECT_TRUE(mesher.had_traversed_layer()); + EXPECT_GT(mesher.traversed_count(), 0u); + const auto m = mesher.extract(-25.0, 0.0, 0.0, params()); + std::size_t flagged = 0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + flagged += m.traversed[t]; + } + EXPECT_GT(flagged, 10u); +} + +TEST(SurfaceLod, RampKeepsItsShape) +{ + SurfaceLodMesher mesher(garage(), config()); + // robot on the ramp: every vertex of the ramp zone is on (or within the cluster error of) it + const auto m = mesher.extract(-15.0, 0.0, 1.5, params()); + for (std::size_t v = 0; v < m.x.size(); ++v) { + if (m.x[v] > -19.0 && m.x[v] < -11.0) { + // the ramp, or the floor under it + const double ramp = 0.3 * (m.x[v] + 20.0); + ASSERT_TRUE(std::abs(m.z[v] - ramp) < 0.65 || std::abs(m.z[v]) < 0.01) + << "x " << m.x[v] << " z " << m.z[v]; + } + } +} + +TEST(SurfaceLod, ABorderKeepsItsPlaceAndANarrowRampItsWidth) +{ + // a ramp 2.4 m wide: clusters of 1 m must not shrink it + ::navmap::NavMap nm; + add_patch(nm, -20, 20, -1.2, 1.2, 0.4, [](double x, double) {return 0.1 * (x + 20.0);}); + SurfaceLodMesher mesher(nm, config()); + for (const double rx : {0.0, -19.0, 19.0}) { + const auto m = mesher.extract(rx, 0.0, 0.1 * (rx + 20.0), params()); + float ymin = 1e9f, ymax = -1e9f; + for (std::size_t v = 0; v < m.y.size(); ++v) { + if (std::abs(m.x[v] - rx) < 8.0) { + ymin = std::min(ymin, m.y[v]); + ymax = std::max(ymax, m.y[v]); + } + } + EXPECT_NEAR(ymin, -1.2f, 0.05f) << "robot at " << rx; + EXPECT_NEAR(ymax, 1.2f, 0.05f) << "robot at " << rx; + } +} + +TEST(SurfaceLod, SlopeComesFromTheSourceNotFromTheSimplification) +{ + SurfaceLodMesher mesher(garage(), config()); + const double ramp_deg = std::atan(0.3) * 180.0 / M_PI; // the steepest thing in the map + for (const auto & pos : {std::array{15, 0, 0}, std::array{-100, 0, 0}, + std::array{-15, 0, 1.5}}) + { + const auto m = mesher.extract(pos[0], pos[1], pos[2], params()); + ASSERT_EQ(m.slope.size(), m.triangle_count()); + float steepest = 0; + for (const auto s : m.slope) { + steepest = std::max(steepest, s); + } + EXPECT_LE(steepest, ramp_deg + 0.5) << "robot at " << pos[0]; + EXPECT_GT(steepest, ramp_deg - 0.5); // and the ramp is there + } +} + +TEST(SurfaceLod, NarrowPassagesKeepTheirResolutionWhateverTheDistance) +{ + // a ramp 2.4 m wide (narrow) and a hall 40 m wide, side by side + ::navmap::NavMap nm; + add_patch(nm, -20, 20, -1.2, 1.2, 0.4, [](double x, double) {return 0.1 * (x + 20.0);}); + add_patch(nm, -20, 20, 10, 50, 0.4, [](double, double) {return 0.0;}); + auto cfg = config(); + cfg.keep_narrow_width = 3.0; + SurfaceLodMesher kept(nm, cfg); + EXPECT_GT(kept.narrow_vertex_count(), 100u); + cfg.keep_narrow_width = 0.0; + SurfaceLodMesher plain(nm, cfg); + EXPECT_EQ(plain.narrow_vertex_count(), 0u); + // robot at the far end of the hall: the ramp is 60 m away + const auto a = kept.extract(0.0, 45.0, 0.0, params()); + const auto b = plain.extract(0.0, 45.0, 0.0, params()); + std::size_t fine_ramp = 0, coarse_ramp = 0, coarse_hall = 0; + for (std::size_t t = 0; t < a.triangle_count(); ++t) { + const auto c = centroid(a, t); + if (std::abs(c.y) < 1.5 && a.leg[t] <= 1.0f) {++fine_ramp;} + } + for (std::size_t t = 0; t < b.triangle_count(); ++t) { + const auto c = centroid(b, t); + if (std::abs(c.y) < 1.5 && b.leg[t] <= 1.0f) {++coarse_ramp;} + if (c.y > 10 && b.leg[t] >= 4.0f) {++coarse_hall;} + } + EXPECT_GT(fine_ramp, 5 * coarse_ramp + 100); // the ramp is at full resolution + EXPECT_GT(coarse_hall, 0u); // the hall is not + // and the hall is as coarse as ever with the option on + std::size_t hall_fine_kept = 0, hall_fine_plain = 0; + for (std::size_t t = 0; t < a.triangle_count(); ++t) { + hall_fine_kept += centroid(a, t).y < 9 ? 0 : (a.leg[t] <= 1.0f && centroid(a, t).y < 25); + } + for (std::size_t t = 0; t < b.triangle_count(); ++t) { + hall_fine_plain += centroid(b, t).y < 9 ? 0 : (b.leg[t] <= 1.0f && centroid(b, t).y < 25); + } + EXPECT_LE(hall_fine_kept, hall_fine_plain + 5); // (a corner or two may count as narrow) +} + +TEST(SurfaceLod, AGapOrALedgeIsNeverBridgedHoweverCoarse) +{ + // two pieces of floor that do not touch: 0.6 m apart (inside one 1 m cell), 5 m apart, and a + // 1 m ledge with a gap. The mesh must not join them at any distance from the robot. + struct Case {double gap0, gap1, z_right;}; + for (const Case c : {Case{0.2, 0.8, 0.0}, Case{1.0, 6.0, 0.0}, Case{0.2, 0.8, 1.0}}) { + ::navmap::NavMap nm; + add_patch(nm, -20.0, c.gap0, -5.0, 5.0, 0.2, [](double, double) {return 0.0;}); + add_patch(nm, c.gap1, 20.0, -5.0, 5.0, 0.2, [&](double, double) {return c.z_right;}); + auto cfg = config(); + cfg.drop_unreachable_ground = false; + const SurfaceLodMesher mesher(nm, cfg); + for (const double rx : {0.5, 40.0, 200.0}) { + const auto m = mesher.extract(rx, 0.0, 0.0, params()); + ASSERT_GT(m.triangle_count(), 0u); + EXPECT_FALSE(connected( + m, [&](const Centroid & q) {return q.x < c.gap0 - 0.5;}, + [&](const Centroid & q) {return q.x > c.gap1 + 0.5;})) + << "gap " << c.gap0 << ".." << c.gap1 << ", ledge " << c.z_right << ", robot at " << rx; + } + } +} + +TEST(SurfaceLod, AMapWithANonFiniteVertexOrABadIndexIsRefusedAndNaNPosesMarkNothing) +{ + // a NaN vertex was welded, cast to a cell index (undefined: INT64_MIN, then an overflow in the + // cell loops) and came out in every mesh; a NaN robot position did the same in mark() + SurfaceLodMesher::Config cfg; + cfg.min_size = 0.5; + const double nan = std::numeric_limits::quiet_NaN(); + for (const float bad : {std::nanf(""), std::numeric_limits::infinity()}) { + auto nm = garage(); + nm.positions.z[7] = bad; + EXPECT_THROW(SurfaceLodMesher(nm, cfg), std::invalid_argument); + nm = garage(); + nm.positions.x[0] = bad; + EXPECT_THROW(SurfaceLodMesher(nm, cfg), std::invalid_argument); + } + { + auto nm = garage(); + nm.navcels[3].v[1] = static_cast(nm.positions.x.size()) + 5; + EXPECT_THROW(SurfaceLodMesher(nm, cfg), std::invalid_argument); + } + SurfaceLodMesher s(garage(), cfg); + EXPECT_EQ(s.mark(nan, 0.0, 0.0, 1.0), 0); + EXPECT_EQ(s.mark(0.0, nan, 0.0, 1.0), 0); + EXPECT_EQ(s.mark(0.0, 0.0, 0.0, nan), 0); + EXPECT_EQ(s.traversed_count(), 0u); + EXPECT_GT(s.mark(0.0, 0.0, 0.0, 1.0), 0); + EXPECT_EQ(s.forget_outside(nan, 0.0, 0.0, 1.0), 0u); // not forgotten around nowhere + EXPECT_GT(s.traversed_count(), 0u); + const auto m = s.extract(nan, nan, nan, params()); + EXPECT_GT(m.triangle_count(), 0u); // a mesh all the same (the policy sees no distance) + for (const auto z : m.z) { + EXPECT_TRUE(std::isfinite(z)); + } +} + +namespace +{ +// A mesh as a sorted list of triangles, each as its three corners (rotated so that the smallest +// comes first, the orientation kept) and what it carries: two meshes with the same triangles in +// another order compare equal +using Corner = std::array; +struct Tri +{ + std::array c; + float leg, slope; + int traversed; + bool operator<(const Tri & o) const + { + return std::tie(c, leg, slope, traversed) < std::tie(o.c, o.leg, o.slope, o.traversed); + } + bool operator==(const Tri & o) const + { + return c == o.c && leg == o.leg && slope == o.slope && traversed == o.traversed; + } +}; + +std::vector canonical(const LodMesh & m) +{ + std::vector out; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + std::array c; + for (int k = 0; k < 3; ++k) { + const auto v = m.tri[3 * t + k]; + c[k] = {m.x[v], m.y[v], m.z[v]}; + } + std::rotate(c.begin(), std::min_element(c.begin(), c.end()), c.end()); + out.push_back({c, m.leg[t], m.slope[t], m.traversed[t]}); + } + std::sort(out.begin(), out.end()); + return out; +} + +// The garage with bumps on the lower floor and holes (obstacles) here and there +::navmap::NavMap holed_garage(unsigned seed) +{ + ::navmap::NavMap nm; + add_patch(nm, -30, 20, -10, 10, 0.5, [](double x, double y) { + return 0.15 * std::sin(0.7 * x) * std::cos(0.9 * y); + }); + add_patch(nm, -20, -10, -10, 10, 0.5, [](double x, double) {return 0.3 * (x + 20.0);}); + add_patch(nm, -10, 20, -10, 10, 0.5, [](double, double) {return 3.0;}); + std::vector<::navmap::NavCel> kept; + unsigned s = seed; + for (const auto & c : nm.navcels) { + s = s * 1103515245u + 12345u; + if ((s >> 16) % 37 != 0) {kept.push_back(c);} + } + nm.navcels = kept; + return nm; +} +} // namespace + +TEST(SurfaceLod, TheIncrementalExtractionIsTheFullOneWhereverTheRobotAndTheGroundAre) +{ + // extract() takes from a precomputed far cut whatever the robot (and the driven-over / + // observed ground) cannot change and climbs again only the rest: the result must be the + // reference's triangle by triangle, for any position, policy and ground + for (unsigned seed : {1u, 7u}) { + for (const double hole_error : {0.0, 0.3}) { + for (const double narrow : {0.0, 1.6}) { + auto cfg = config(); + cfg.hole_error = hole_error; + cfg.keep_narrow_width = narrow; + SurfaceLodMesher mesher(holed_garage(seed), cfg); + auto p = params(); + unsigned s = seed; + const auto rnd = [&s](double lo, double hi) { + s = s * 1103515245u + 12345u; + return lo + (hi - lo) * ((s >> 8) % 100000) / 100000.0; + }; + for (int i = 0; i < 12; ++i) { + const double x = rnd(-35.0, 25.0), y = rnd(-12.0, 12.0); + const double z = rnd(0.0, 1.0) < 0.5 ? 0.0 : 3.0; + if (i == 4) {mesher.mark(x, y, z, 2.0);} + if (i == 6) {mesher.mark_observed(x, y, z, 6.0);} + if (i == 7) {p.observed_margin = 0.5;} + if (i == 8) {p.high_res_radius = 4.0; p.known_falloff = 1.0;} // the far cut stays + if (i == 9) {p.max_size = 8.0; p.error_growth = 0.0;} // it is rebuilt + if (i == 10) {mesher.forget_outside(x, y, z, 15.0);} + if (i == 11) {mesher.forget_observed_outside(x, y, z, 0.0);} + const auto fast = mesher.extract(x, y, z, p); + const auto full = mesher.extract_reference(x, y, z, p); + ASSERT_GT(full.triangle_count(), 0u); + ASSERT_EQ(fast.triangle_count(), full.triangle_count()) << "seed " << seed << " i " << i; + ASSERT_TRUE(canonical(fast) == canonical(full)) << "seed " << seed << " i " << i; + ASSERT_EQ(fast.x.size(), full.x.size()); + ASSERT_EQ(fast.r.size(), full.r.size()); + } + } + } + } +} + +TEST(SurfaceLod, AHoleSeenFromNearKeepsItsShapeFromFarWhenObserved) +{ + // A square hole of 1 m in the middle of a flat floor. The hierarchy never bridges it, but from + // far the clusters around it are coarse and its shape with them; once its border was observed + // it is kept at full resolution wherever the robot goes + ::navmap::NavMap nm; + add_patch(nm, -40, 40, -10, 10, 0.5, [](double, double) {return 0.0;}); + std::vector<::navmap::NavCel> kept; + for (const auto & c : nm.navcels) { + double cx = 0, cy = 0; + for (int k = 0; k < 3; ++k) { + cx += nm.positions.x[c.v[k]] / 3.0; + cy += nm.positions.y[c.v[k]] / 3.0; + } + if (std::abs(cx - 20.25) < 0.5 && std::abs(cy - 0.25) < 0.5) {continue;} + kept.push_back(c); + } + nm.navcels = kept; + // the largest triangle with a corner within 1 m of the hole's centre + const auto coarsest_at_hole = [](const LodMesh & m) { + float worst = 0.0f; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + for (int k = 0; k < 3; ++k) { + const auto v = m.tri[3 * t + k]; + if (std::hypot(m.x[v] - 20.25, m.y[v] - 0.25) < 1.0) {worst = std::max(worst, m.leg[t]);} + } + } + return worst; + }; + auto p = params(); + SurfaceLodMesher mesher(nm, config()); + EXPECT_LE(coarsest_at_hole(mesher.extract(20.0, 0.0, 0.0, p)), 1.01f); + EXPECT_GE(coarsest_at_hole(mesher.extract(-35.0, 0.0, 0.0, p)), 4.0f); // 55 m away + EXPECT_GT(mesher.mark_observed(20.0, 0.0, 0.0, 3.0), 0); + EXPECT_EQ(mesher.mark_observed(20.0, 0.0, 0.0, 3.0), 0); + EXPECT_GT(mesher.observed_count(), 0u); + EXPECT_EQ(mesher.traversed_count(), 0u); // observed is not driven over + EXPECT_LE(coarsest_at_hole(mesher.extract(-35.0, 0.0, 0.0, p)), 1.01f); + EXPECT_GT(mesher.forget_observed_outside(-35.0, 0.0, 0.0, 10.0), 0u); + EXPECT_GE(coarsest_at_hole(mesher.extract(-35.0, 0.0, 0.0, p)), 4.0f); + + // a hole_error above the tolerance at that distance keeps its border where the map has it with + // nothing observed: the vertices around the hole are the map's (on its 0.5 m lattice), not the + // means of border clusters + const auto border_is_the_maps = [](const LodMesh & m) { + for (std::size_t v = 0; v < m.x.size(); ++v) { + if (std::hypot(m.x[v] - 20.25, m.y[v] - 0.25) > 1.0) {continue;} + if (std::abs(m.x[v] * 2.0f - std::round(m.x[v] * 2.0f)) > 1e-3f || + std::abs(m.y[v] * 2.0f - std::round(m.y[v] * 2.0f)) > 1e-3f) + { + return false; + } + } + return true; + }; + EXPECT_FALSE(border_is_the_maps(mesher.extract(-35.0, 0.0, 0.0, p))); + auto cfg = config(); + cfg.hole_error = 5.0; + SurfaceLodMesher strict(nm, cfg); + EXPECT_TRUE(border_is_the_maps(strict.extract(-35.0, 0.0, 0.0, p))); +} + +TEST(SurfaceLod, BuildsFromTheArraysOfAMapAsFromTheMapAndReportsItsMemory) +{ + const auto nm = garage(); + SurfaceLodMesher a(nm, config()); + SurfaceLodMesher b(bigmap_maps_manager::SourceMesh::from_navmap(nm), config()); + EXPECT_EQ(a.vertex_count(), b.vertex_count()); + EXPECT_TRUE(canonical(a.extract(0, 0, 0, params())) == canonical(b.extract(0, 0, 0, params()))); + EXPECT_GT(a.memory_bytes(), a.vertex_count() * 12); + // far less than the NavMap it comes from (positions, NavCels, normals, adjacency, BVH...) + EXPECT_LT(a.memory_bytes(), 200 * a.source_triangle_count()); +} + +// Full resolution within the fine radius (paper, Section 3.4): guaranteed on a flat floor; on a +// ramp the clusters may be coarser. The longest edge of the triangles near the robot on its own +// surface, for the robot on the upper floor and on the ramp (the ramp's value is reported). +TEST(SurfaceLod, TheFineDiscIsAtFullResolutionOnAFloorAndIsMeasuredOnARamp) +{ + SurfaceLodMesher mesher(garage(), config()); + const auto p = params(); + auto longest_near = [&](double rx, double ry, double rz, double radius, std::size_t & n) { + const auto m = mesher.extract(rx, ry, rz, p); + double longest = 0.0; + n = 0; + for (std::size_t t = 0; t < m.triangle_count(); ++t) { + const auto c = centroid(m, t); + if (std::hypot(c.x - rx, c.y - ry) > radius || std::abs(c.z - rz) > 1.0) {continue;} + ++n; + for (int k = 0; k < 3; ++k) { + const auto a = m.tri[3 * t + k], b = m.tri[3 * t + (k + 1) % 3]; + longest = std::max(longest, static_cast(std::hypot( + std::hypot(m.x[a] - m.x[b], m.y[a] - m.y[b]), m.z[a] - m.z[b]))); + } + } + return longest; + }; + std::size_t n_floor = 0, n_ramp = 0; + // the fine disc minus a margin of one coarse edge (triangles straddling its border may be larger) + const double r = p.high_res_radius - 2.0 * config().min_size; + const double floor = longest_near(10.0, 0.0, 3.0, r, n_floor); + const double ramp = longest_near(-15.0, 0.0, 1.5, r, n_ramp); + std::printf("[fine disc] longest edge within %.1f m: floor %.2f m (%zu triangles), ramp %.2f m " + "(%zu triangles); min_size %.2f m\n", r, floor, n_floor, ramp, n_ramp, config().min_size); + ASSERT_GT(n_floor, 0u); + ASSERT_GT(n_ramp, 0u); + EXPECT_LE(floor, config().min_size * std::sqrt(2.0) + 1e-3); + RecordProperty("ramp_longest_edge_m", std::to_string(ramp)); +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_update_watchdog.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_update_watchdog.cpp new file mode 100644 index 00000000..71fe22cb --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_update_watchdog.cpp @@ -0,0 +1,80 @@ +// Copyright 2026 José Miguel Guerrero +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "easynav_bigmap_maps_manager/update_watchdog.hpp" + +using bigmap_maps_manager::UpdateWatchdog; +using Event = UpdateWatchdog::Event; + +TEST(UpdateWatchdog, NothingIsToldBeforeItStartsOrWhileUpdateRuns) +{ + UpdateWatchdog w(10.0, 30.0); + EXPECT_EQ(w.check(1000.0), Event::None); // not started: the plugin is not initialized yet + w.start(0.0); + for (double t = 1.0; t < 100.0; t += 1.0) { + w.ran(t); + EXPECT_EQ(w.check(t + 0.5), Event::None) << t; + } +} + +TEST(UpdateWatchdog, AClockThatNeverMovesIsAStallFromTheStart) +{ + // the case seen end to end: update() never runs at all + UpdateWatchdog w(10.0, 30.0); + w.start(100.0); + EXPECT_EQ(w.check(105.0), Event::None); // within the grace + EXPECT_EQ(w.check(110.5), Event::Stalled); + EXPECT_DOUBLE_EQ(w.stalled_s(), 10.5); + EXPECT_EQ(w.check(115.0), Event::None); // told once... + EXPECT_EQ(w.check(140.5), Event::Stalled); // ...and again every repeat_s while it goes on + EXPECT_DOUBLE_EQ(w.stalled_s(), 40.5); +} + +TEST(UpdateWatchdog, AStallThatEndsIsToldOnceAndThenItWatchesAgain) +{ + UpdateWatchdog w(10.0, 30.0); + w.start(0.0); + w.ran(5.0); + EXPECT_EQ(w.check(20.0), Event::Stalled); + w.ran(21.0); + EXPECT_EQ(w.check(21.5), Event::Resumed); + EXPECT_EQ(w.check(22.0), Event::None); + // a second stall is a new one (told after the grace, not after repeat_s) + EXPECT_EQ(w.check(31.5), Event::Stalled); +} + +TEST(UpdateWatchdog, RunsFromAnotherThreadAreSeen) +{ + // update() and the wall timer run on different threads: the checker sees the latest run + UpdateWatchdog w(0.5, 30.0); + w.start(0.0); + std::atomic last{0.0}; + std::thread updater([&] { + for (int i = 1; i <= 100000; ++i) { + w.ran(i * 0.001); + last.store(i * 0.001); + } + }); + while (last.load() < 50.0) { + EXPECT_NE(w.check(last.load() + 0.1), Event::Stalled); + } + updater.join(); + EXPECT_EQ(w.check(100.1), Event::None); + EXPECT_EQ(w.check(101.0), Event::Stalled); +}