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..d8daf345 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 NavMaps: fine around the robot and coarse far from it (tiles at levels of detail), with the sensed obstacles near it. | [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..e825be8a --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/CMakeLists.txt @@ -0,0 +1,100 @@ +cmake_minimum_required(VERSION 3.8) +project(easynav_bigmap_maps_manager) + +# Without a build type colcon compiles with no optimisation, and simplifying a large map takes +# minutes instead of seconds. 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(pluginlib REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_sensors REQUIRED) +find_package(easynav_navmap_maps_manager REQUIRED) +find_package(navmap_core REQUIRED) +find_package(navmap_ros REQUIRED) +find_package(navmap_ros_interfaces REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) + +# The level-of-detail engine: ROS-free +add_library(${PROJECT_NAME}_lod SHARED + src/tiled_lod.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 +) +# 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 + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + pluginlib::pluginlib + easynav_common::easynav_common + easynav_core::easynav_core + easynav_sensors::easynav_sensors + navmap_ros::navmap_ros + ${navmap_ros_interfaces_TARGETS} + ${nav_msgs_TARGETS} + ${std_msgs_TARGETS} + ${visualization_msgs_TARGETS} +) +pluginlib_export_plugin_description_file(easynav_core easynav_bigmap_maps_manager_plugins.xml) + +install( + TARGETS ${PROJECT_NAME}_lod ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_tiled_lod test/test_tiled_lod.cpp) + target_link_libraries(test_tiled_lod ${PROJECT_NAME}_lod) + ament_add_gtest(test_bigmap_plugin test/test_bigmap_plugin.cpp TIMEOUT 300) + target_link_libraries(test_bigmap_plugin ${PROJECT_NAME}) +endif() + +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp rclcpp_lifecycle pluginlib nav_msgs std_msgs visualization_msgs + easynav_common easynav_core easynav_sensors easynav_navmap_maps_manager + navmap_core navmap_ros navmap_ros_interfaces Eigen3) +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..b4f6ac78 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/README.md @@ -0,0 +1,132 @@ +# easynav_bigmap_maps_manager + +A maps manager for NavMaps too big to plan on at full detail (a warehouse, a town): the map is +fine around the robot and coarse far from it, and the obstacles the sensors see are added near the +robot and forgotten when it is far from them. + +Planning on it, against planning on the full map: 2 x 2 km around the Puerta del Sol, Madrid +(ground from the IGN, buildings from the Catastro, 13.8 M NavCels at 0.5 m), 30 routes of 230 to +1481 m, the same A* and safety band on every map, the robot replanning every 30 m on the map made +around it (the `city_routes` campaign of +[bigmap_experiments](https://github.com/jmguerreroh/bigmap_experiments)): + +| Map | NavCels | MB | Success | Through a building | Length | A* per plan | NavCels expanded | +|---|---|---|---|---|---|---|---| +| full, 0.5 m | 13,765,636 | 321 | 30/30 | 0 | 1.000 | 351 ms | 1,166,402 | +| grid 1 m | 3,275,294 | 77 | 21/30 | 0 | 1.006 | 58 ms | 190,060 | +| grid 2 m | 744,768 | 18 | 28/30 | 0 | 1.008 | 16 ms | 71,510 | +| grid 4 m | 156,118 | 3.9 | 13/30 | 0 | 1.354 | 6.7 ms | 24,202 | +| full map simplified once (0.4 m) | 393,251 | 10.3 | 30/30 | 0 | 1.022 | 14 ms | 35,082 | +| bigmap (0.8 to 8 m, 20 to 300 m) | ~135,000 | 3.5 | 30/30 | 0 | 1.007 | 5.6 ms | 8,528 | + +Paths within 1 % of those on the full map, planned 63 times faster, expanding 137 times fewer +NavCels, on a map 92 times smaller, made again (with the planner's graph) in 226 ms every 30 m. +Coarser fixed grids close streets (1, 2 and 4 m fail routes) or go around them (4 m: 35 % longer); +the map simplified once as a whole finds every route, 2.5 times slower to plan on. + +## Configuration + +```yaml +maps_manager_node: + ros__parameters: + map_types: [bigmap] + bigmap: + plugin: easynav_bigmap_maps_manager/BigMapMapsManager + package: easynav_large_warehouse # optional: navmap_path_file is relative to it + navmap_path_file: maps/large_warehouse_collision.navmap + cell_size: + min: 0.5 # m: NavCels at the obstacles near the robot + max: 8.0 # m: the largest NavCel; far away the map is within max / 2 + min_radius: 15.0 # m: within it, the obstacles have NavCels of cell_size.min + max_radius: 60.0 # m: from it, the map is as coarse as it gets + forget_distance: 60.0 # m: sensed obstacles farther than this are forgotten (default max_radius) + filters: [inflation] + inflation: + plugin: easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter + inflation_radius: 2.0 + cost_scaling_factor: 1.5 +``` + +That is all it needs. A few more parameters have defaults that suit most robots: + +| Parameter | Default | | +|---|---|---| +| `obstacle_max_height` | 2.0 m | higher points do not touch the robot: set it to the robot's height (e.g. so that the shelves of a rack it passes under are not obstacles) | +| `obstacle_min_height` | 0.1 m | lower points above the map's surface are the ground | +| `obstacle_min_height_per_meter` | 0.02 | `obstacle_min_height` grows this much per metre from the robot: a slight tilt of the robot or the sensor puts the far ground above it | +| `sensor_range` | `min_radius` | points farther from the robot are not used (at most `forget_distance`) | +| `rebuild_period` | 0.5 s | the map is made again at most this often | +| `memory.clearing` | true | the rays from the robot to what it sees clear the sensed obstacles they cross (one that moves away stops blocking) | +| `memory.p_hit`, `memory.p_miss` | 0.7, 0.4 | probability that a cell is an obstacle after a point on it, and after a ray through it (as OctoMap): how many sightings mark it, and clear it | +| `memory.decay_time` | 0 s | a sensed obstacle not seen again for this long is forgotten (0: only `forget_distance` and the clearing forget it) | + +The filters are those of `easynav_navmap_maps_manager`, run on the map every cycle. The inflation +radius also sets how far around an obstacle the NavCels are fine (see below). + +## How + +**Once, at start** (seconds, on every core: 9 s for the 2.5 M NavCels of the warehouse, 45 s for the +14 M of 2 x 2 km of Madrid, with cells of 0.5 to 16 m in the benchmark of bigmap_experiments): + +1. The NavMap is cut along a grid (`navmap::cut_along_lines`) into square tiles of `min_radius / 2`. +2. Each tile is simplified (`navmap::simplify`) within `cell_size.min / 2`, no edge longer than + `cell_size.max`: level 0. +3. Level k: four tiles of level k - 1 put together and simplified within twice the error, up to + `cell_size.max / 2` (a quadtree; 0.5 / 1 / 2 / 4 / 8 m are five levels). + +`simplify` keeps the map's topology (no hole closes, no wall opens, however coarse) and its layers. +The corners of a tile and the points where the map's border leaves its sides are kept +(`SimplifyParams::keep_vertices`), so its sides only lose vertices along them. + +**Every cycle**, when the robot has moved a few metres or the sensed obstacles changed (at most +twice a second): + +1. **Select** the coarsest tile each place allows: level 0 within `min_radius`, the coarsest from + `max_radius`, geometric in between. +2. **Assemble** them in one NavMap. Where a tile has a vertex on a side its neighbour does not, + the neighbour's NavCel there is split to reach it, so the map is conforming (no cracks) and the + planner crosses from tile to tile. +3. **Detail** (`navmap::refine`, longest-edge bisection): within `min_radius`, the borders of the + map (walls, racks, the edges of the holes) and the sensed obstacles get NavCels of + `cell_size.min`, and so does every NavCel within the inflation radius of them (the cost of a + NavCel is that of its centroid: a large one next to an obstacle would read as free). Free + ground away from them stays as large as it was. Farther obstacles get the cell size of their + place. +4. The sensed obstacles are marked `LETHAL_OBSTACLE` in the layer `obstacles`; the filters run. + +| | Warehouse (2.5 M NavCels) | Madrid, 2 x 2 km (13.8 M) | +|---|---|---| +| Map around the robot | ~27 k NavCels (~35 k with the detail) | ~157 k | +| Assemble | 7 ms | 45-65 ms | +| Detail | 30 ms | 70-190 ms | + +**Sensed obstacles**: the points of the perceptions within `sensor_range`, in the robot's footprint +frame, between `obstacle_min_height` (plus `obstacle_min_height_per_meter` per metre) and +`obstacle_max_height` above the map's surface under them. A point over no surface, or within +`cell_size.min` of a place with none (a rack, a building, seen from the side), is the map's own +obstacle and is left out. They are remembered by cells of `cell_size.min / 2` as evidence in +log-odds, as the NavMap maps manager's obstacle memory: a point on a cell is a hit, a ray from the +robot through it (to the ground or to an obstacle beyond) a miss, so an obstacle that moves away is +cleared once the robot sees through where it was. A cell is an obstacle while it is more likely +occupied than not; it is forgotten when the robot is `forget_distance` from it (or, with +`memory.decay_time`, when it is not seen again for that long). + +## Topics + +| Topic | Type | | +|---|---|---| +| `//map` | `navmap_ros_interfaces/msg/NavMap` | the map, when it changes (at most once a second) | +| `//map_updates` | `navmap_ros_interfaces/msg/NavMapLayer` | what the filters change, as the NavMap maps manager | +| `//stats` | `std_msgs/msg/Float64MultiArray` | every cycle, while someone listens: time (s), rebuilds so far, the last rebuild's select, assemble and detail (ms) and NavCels, this cycle's sensing and filters (ms), sensed obstacles | + +## From NavMap + +It is built on NavMap's tools: `simplify` (with `keep_vertices`), `cut_along_lines` and `refine` +(`navmap_core/Subdivide.hpp`). + +## Tests + +`colcon test --packages-select easynav_bigmap_maps_manager`: the engine (`test_tiled_lod`: levels, +no cracks, holes and narrow passages kept far away, layers) and the plugin as EasyNav runs it +(`test_bigmap_plugin`: fine at the borders near the robot and coarse far, the map follows the robot, +a sensed obstacle is added fine, inflated and forgotten far away, and cleared when it moves away). 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..f23a0e3b --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/easynav_bigmap_maps_manager_plugins.xml @@ -0,0 +1,9 @@ + + + + + Maps manager for large NavMaps: fine around the robot and coarse far from it (tiles simplified at several levels of detail), with the sensed obstacles near the robot. + + + + 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..7aa5b6de --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/BigMapMapsManager.hpp @@ -0,0 +1,119 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +/** + * @file BigMapMapsManager.hpp + * @brief A large NavMap, fine around the robot and coarse far from it. + * + * Loads a NavMap, cuts it into tiles and simplifies them at several levels of detail (TiledLod, + * once). Every cycle it puts together, around the robot, the coarsest tile each place allows; + * near the robot it refines the borders of the map and the obstacles the sensors see (which it + * remembers until the robot is forget_distance away from them). The result is "map.navmap"; + * the filters (e.g. inflation) run on it as in the NavMap maps manager. + */ + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__BIGMAPMAPSMANAGER_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__BIGMAPMAPSMANAGER_HPP_ + +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/sensed_obstacles.hpp" +#include "easynav_bigmap_maps_manager/tiled_lod.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_core/MapsManagerBase.hpp" +#include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" +#include "navmap_core/NavMap.hpp" +#include "navmap_ros_interfaces/msg/nav_map.hpp" +#include "navmap_ros_interfaces/msg/nav_map_layer.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace easynav +{ +namespace bigmap +{ + +class BigMapMapsManager : public easynav::MapsManagerBase +{ +public: + BigMapMapsManager(); + ~BigMapMapsManager() override; + + void on_initialize() override; + void update(::easynav::NavState & nav_state) override; + + /// The level-of-detail engine (for tests) + const TiledLod & lod() const {return lod_;} + /// The obstacles remembered (for tests) + const SensedObstacles & sensed() const {return sensed_;} + +private: + /// Robot position (map frame) and heading; false if unknown + bool robot_pose(const ::easynav::NavState & nav_state, Eigen::Vector3d & p, double & yaw) const; + /// The obstacles in this cycle's perceptions, into sensed_. True if any is new. + bool sense(const ::easynav::NavState & nav_state, const Eigen::Vector3d & robot, double yaw); + /// The map around the robot, into map_ + void rebuild(const std::vector & tiles, const Eigen::Vector3d & robot); + /// The map (as the filters left it) and the markers of its levels + void publish(const ::navmap::NavMap & nm); + + TiledLod lod_; + SensedObstacles sensed_; + ::navmap::NavMap map_; + bool has_map_{false}; + bool map_changed_{false}; + std::vector tiles_; ///< those in map_ + Eigen::Vector2d built_at_{0.0, 0.0}; ///< robot position when map_ was made + double built_z_{0.0}; + double last_build_{-1e18}; ///< s + double last_publish_{-1e18}; ///< s + bool publish_pending_{false}; + bool sensed_changed_{false}; + /// For measurements, in the NavState as "map.bigmap_stats" every cycle: rebuilds so far, and + /// the last rebuild's select, assemble, detail (ms) and NavCels; this cycle's sensing and + /// filters (ms) + std::vector stats_ = std::vector(7, 0.0); + double select_ms_{0.0}; + + // Parameters + double forget_distance_{60.0}; ///< m: the sensed obstacles farther than this are forgotten + double margin_{0.0}; ///< m: fine around obstacles (the filters' inflation radius) + float known_{0.5f}; ///< m: a point this close to no ground is the map's obstacle + double min_height_{0.1}; ///< m above the map's surface: lower points are the ground + double min_height_per_m_{0.02}; ///< m: min_height grows this much per metre from the robot + double max_height_{2.0}; ///< m above it: higher points do not touch the robot + double sensor_range_{15.0}; ///< m: points farther from the robot are not used + double rebuild_period_{0.5}; ///< s: the map is made again at most this often + double publish_period_{1.0}; ///< s: and published at most this often + + std::unique_ptr> filters_loader_; + std::vector> filters_; + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr layer_updates_pub_; + rclcpp::Publisher::SharedPtr markers_pub_; + /// stats_ every cycle, with the time (s) first: what a cycle costs, for measurements + rclcpp::Publisher::SharedPtr stats_pub_; + std::size_t last_subscribers_{0}; +}; + +} // 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/sensed_obstacles.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/sensed_obstacles.hpp new file mode 100644 index 00000000..67a052e5 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/sensed_obstacles.hpp @@ -0,0 +1,236 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +/** + * @file sensed_obstacles.hpp + * @brief The obstacles the sensors have seen that the map does not have, as an occupancy memory + * by cells (one point each, on the ground under the obstacle): evidence in log-odds (a + * point above the ground is a hit, a ray from the robot through a cell is a miss), so that + * what moves away is cleared; forgotten when the robot is far, or unseen for a while. + */ + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__SENSED_OBSTACLES_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__SENSED_OBSTACLES_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" + +namespace easynav +{ +namespace bigmap +{ + +/// How the evidence accumulates (as OctoMap): probabilities of a hit and of a miss, the clamp. +struct MemoryParams +{ + double p_hit{0.7}; + double p_miss{0.4}; + double p_min{0.12}; + double p_max{0.97}; + bool clearing{true}; ///< rays from the robot to each point clear the cells they cross + double decay_time{0.0}; ///< s: a cell not seen again for this long is forgotten; 0: never +}; + +class SensedObstacles +{ +public: + /// A ray of the sensor, on the map's surface: from the robot's ground to the ground under the + /// point (`end`); `end_free`: the point is the ground itself, so its cell is free too. + struct Ray + { + Eigen::Vector3f end; + bool end_free; + }; + + /// Cells of `cell` m on the plane (1 m in height, so floors one over another stay apart) + explicit SensedObstacles(double cell = 0.5, const MemoryParams & p = {}) + : cell_(cell), p_(p) + { + auto logodds = [](double p) {return static_cast(std::log(p / (1.0 - p)));}; + l_hit_ = logodds(p_.p_hit); + l_miss_ = logodds(p_.p_miss); + l_min_ = logodds(p_.p_min); + l_max_ = logodds(p_.p_max); + } + + /// One cycle of evidence: `hits` (obstacles, on the ground under them) and, with clearing, the + /// rays from `origin` (the robot's ground). Each cell gets one hit or one miss per cycle (a hit + /// beats a miss); unknown cells crossed by a ray are not stored. True if the obstacles changed. + bool update( + const std::vector & hits, const Eigen::Vector3f & origin, + const std::vector & rays, double now) + { + bool changed = false; + std::unordered_set hit; + for (const auto & q : hits) { + const auto k = key(q); + if (!hit.insert(k).second) {continue;} + auto [it, fresh] = cells_.try_emplace(k, Cell{q, 0.0f, now}); + const bool was = it->second.l > 0.0f; + it->second.l = std::min(it->second.l + l_hit_, l_max_); + it->second.q = q; + it->second.seen = now; + changed |= fresh || (!was && it->second.l > 0.0f); + } + if (p_.clearing && !cells_.empty()) { + std::unordered_set missed; + // a cell is crossed if the ray passes within half a metre of its height (the ray is + // straight, the ground under it is not; floors are farther apart than that) + for (const auto & r : rays) { + walk(origin, r.end, r.end_free, [&](std::int64_t i, std::int64_t j, double z) { + const auto k0 = static_cast(std::floor(z)); + for (std::int64_t k = k0 - 1; k <= k0 + 1; ++k) { + const auto c = key(i, j, k); + const auto it = cells_.find(c); + if (it != cells_.end() && !hit.count(c) && std::abs(it->second.q.z() - z) <= 0.5) { + missed.insert(c); + } + } + }); + } + for (const auto k : missed) { + auto it = cells_.find(k); + const bool was = it->second.l > 0.0f; + it->second.l = std::max(it->second.l + l_miss_, l_min_); + if (it->second.l <= 0.0f) { // free now: not stored + cells_.erase(it); + changed |= was; + } + } + } + if (p_.decay_time > 0.0) { + for (auto it = cells_.begin(); it != cells_.end(); ) { + if (now - it->second.seen > p_.decay_time) { + changed |= it->second.l > 0.0f; + it = cells_.erase(it); + } else { + ++it; + } + } + } + return changed; + } + + /// Forgets the obstacles farther than d (m, on the plane) from (x, y). How many. + std::size_t forget_beyond(double x, double y, double d) + { + std::size_t n = 0; + for (auto it = cells_.begin(); it != cells_.end(); ) { + if (std::hypot(it->second.q.x() - x, it->second.q.y() - y) > d) { + it = cells_.erase(it); + ++n; + } else { + ++it; + } + } + return n; + } + + /// The obstacles: the cells more likely occupied than not + std::vector points() const + { + std::vector out; + out.reserve(cells_.size()); + for (const auto & kv : cells_) { + if (kv.second.l > 0.0f) {out.push_back(kv.second.q);} + } + return out; + } + + std::size_t size() const + { + return static_cast(std::count_if( + cells_.begin(), cells_.end(), [](const auto & kv) {return kv.second.l > 0.0f;})); + } + void clear() {cells_.clear();} + const MemoryParams & params() const {return p_;} + +private: + struct Cell + { + Eigen::Vector3f q; // the latest point seen in it + float l; // log-odds + double seen; // when it was last hit + }; + + std::int64_t key(const Eigen::Vector3f & q) const + { + return key( + static_cast(std::floor(q.x() / cell_)), + static_cast(std::floor(q.y() / cell_)), + static_cast(std::floor(q.z()))); + } + + static std::int64_t key(std::int64_t i, std::int64_t j, std::int64_t k) + { + return ((i & 0x1fffff) << 42) | ((j & 0x1fffff) << 21) | (k & 0x1fffff); + } + + /// The cells a segment crosses on the plane (Amanatides-Woo), f(i, j, the segment's height + /// there); the last one only if `with_end` + template + void walk(const Eigen::Vector3f & a, const Eigen::Vector3f & b, bool with_end, F f) const + { + const double ax = a.x() / cell_, ay = a.y() / cell_, bx = b.x() / cell_, by = b.y() / cell_; + std::int64_t i = static_cast(std::floor(ax)); + std::int64_t j = static_cast(std::floor(ay)); + const auto ie = static_cast(std::floor(bx)); + const auto je = static_cast(std::floor(by)); + const double dx = bx - ax, dy = by - ay; + const int si = dx > 0 ? 1 : -1, sj = dy > 0 ? 1 : -1; + const double inf = std::numeric_limits::infinity(); + double tx = dx != 0.0 ? ((si > 0 ? i + 1 - ax : ax - i) / std::abs(dx)) : inf; + double ty = dy != 0.0 ? ((sj > 0 ? j + 1 - ay : ay - j) / std::abs(dy)) : inf; + const double ddx = dx != 0.0 ? 1.0 / std::abs(dx) : inf; + const double ddy = dy != 0.0 ? 1.0 / std::abs(dy) : inf; + const std::size_t steps = + static_cast(std::abs(ie - i) + std::abs(je - j)); + double t = 0.0; + for (std::size_t n = 0; n <= steps; ++n) { + const bool last = (i == ie && j == je); + if (last && !with_end) {break;} + const double z = a.z() + (b.z() - a.z()) * std::min(t, 1.0); + f(i, j, z); + if (last) {break;} + if (tx < ty) { + t = tx; + tx += ddx; + i += si; + } else { + t = ty; + ty += ddy; + j += sj; + } + } + } + + double cell_; + MemoryParams p_; + float l_hit_, l_miss_, l_min_, l_max_; + std::unordered_map cells_; +}; + +} // namespace bigmap +} // namespace easynav + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__SENSED_OBSTACLES_HPP_ diff --git a/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/tiled_lod.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/tiled_lod.hpp new file mode 100644 index 00000000..96c5ed4b --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/tiled_lod.hpp @@ -0,0 +1,134 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +/** + * @file tiled_lod.hpp + * @brief A NavMap at several levels of detail, by tiles: fine near a point, coarse far from it. + * + * build() cuts the map along a grid (navmap::cut_along_lines) into square tiles and simplifies + * them (navmap::simplify) into a quadtree: a tile of level k is its four children of level k - 1, + * put together and simplified with twice their error. The borders of a tile keep the vertices + * where they meet the borders of the map and its corners (SimplifyParams::keep_vertices), so + * tiles simplified apart still meet. Done once. + * + * select() picks, around a point, the coarsest tile each place allows (level k from radius(k) + * on), and assemble() puts them in one NavMap, stitching their borders: where a tile has a + * vertex on a border its neighbour does not, the neighbour's NavCel there is split to reach it. + * The result is conforming (no cracks), so a planner crosses from tile to tile. Milliseconds. + */ + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__TILED_LOD_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__TILED_LOD_HPP_ + +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +namespace easynav +{ +namespace bigmap +{ + +struct LodParams +{ + double cell_min{0.5}; ///< m: the finest level keeps the map within cell_min / 2 + double cell_max{8.0}; ///< m: no NavCel side longer than this; the coarsest level's error + double min_radius{15.0}; ///< m: the finest level within it + double max_radius{60.0}; ///< m: the coarsest level from it + double tile{0.0}; ///< m: side of the finest tiles; 0: min_radius / 2 +}; + +class TiledLod +{ +public: + struct Stats + { + std::vector navcels; ///< per level, in all its tiles + std::size_t tiles{0}; ///< finest tiles with ground + double seconds{0.0}; + }; + + /// Cuts @p map into tiles and simplifies every level of them (on @p threads threads). The map + /// is taken by value: pass it with std::move when it is no longer needed, so that it is cut in + /// place instead of copied (a large map would be in memory twice). + Stats build(::navmap::NavMap map, const LodParams & params, unsigned threads = 1); + + bool empty() const {return nodes_.empty();} + int levels() const {return top_ + 1;} + /// Cell size of a level (its error is half of it) + double cell(int level) const; + /// From this distance (m, on the plane) a tile may be of this level + double radius(int level) const; + /// The level of the places at distance d (m) from the robot + int level_at(double d) const; + const LodParams & params() const {return p_;} + + /// The tiles to put together around (x, y): the coarsest each place allows + std::vector select(double x, double y) const; + /// Those tiles in one NavMap, their borders stitched (without geometry accelerations if + /// @p accels is false: detail() builds them) + ::navmap::NavMap assemble(const std::vector & tiles, bool accels = true) const; + + /** + * @brief The detail of the obstacles around (x, y), on a map assemble() made there. + * + * Within min_radius, the borders of the map (walls, drops, the edges of the holes) and the + * @p obstacles (points on the surface, e.g. sensed) are refined (navmap::refine) to cell_min, + * and so is every NavCel within @p margin (m; at least cell_min) of them: what an inflation of + * that radius reads. Farther, the obstacles get the cell size of their place. The NavCels of + * the obstacles are marked LETHAL_OBSTACLE in the layer "obstacles" (created if absent). The + * geometry accelerations of @p nm are built at the end. + */ + void detail( + ::navmap::NavMap & nm, double x, double y, const std::vector & obstacles, + double margin = 0.0) const; + + /// Level of a tile + int level(std::uint32_t tile) const {return nodes_[tile].level;} + /// Bounds of a tile: x0, x1, y0, y1 (m) + std::array bounds(std::uint32_t tile) const + { + const auto & l = nodes_[tile].lines; + return {xs_[l[0]], xs_[l[1]], ys_[l[2]], ys_[l[3]]}; + } + +private: + struct Node + { + int level{0}; + std::array lines{0, 0, 0, 0}; ///< its borders: xs_[x0], xs_[x1], ys_[y0], ys_[y1] + std::array child{-1, -1, -1, -1}; + ::navmap::NavMap mesh; + }; + + ::navmap::NavMap stitch(const std::vector & nodes, bool accels) const; + std::vector<::navmap::PointId> border_locks(const Node & n) const; + void simplify_node(Node & n) const; + double distance(const Node & n, double x, double y) const; + + LodParams p_; + int top_{0}; + std::vector xs_, ys_; // grid lines, increasing + std::vector nodes_; + std::vector roots_; +}; + +} // namespace bigmap +} // namespace easynav + +#endif // EASYNAV_BIGMAP_MAPS_MANAGER__TILED_LOD_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..ec57ab3f --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/package.xml @@ -0,0 +1,35 @@ + + + + easynav_bigmap_maps_manager + 0.5.1 + Easy Navigation: BigMap maps manager. A NavMap too big to plan on at full detail, made fine around the robot and coarse far from it: tiles simplified at several levels of detail, put together around the robot, with the obstacles the sensors see refined near it and forgotten far from it. + José Miguel Guerrero + Apache-2.0 + José Miguel Guerrero + + ament_cmake + + rclcpp + rclcpp_lifecycle + pluginlib + nav_msgs + std_msgs + visualization_msgs + easynav_common + easynav_core + easynav_sensors + easynav_navmap_maps_manager + navmap_core + navmap_ros + navmap_ros_interfaces + 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..46aecb37 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -0,0 +1,479 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 +#if defined(__GLIBC__) +#include +#endif +#include +#include +#include +#include +#include + +#include "easynav_common/PackageShare.hpp" +#include "easynav_common/Parameters.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "nav_msgs/msg/odometry.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 +{ + +BigMapMapsManager::BigMapMapsManager() +{ + filters_loader_ = std::make_unique>( + "easynav_navmap_maps_manager", "easynav::navmap::NavMapFilter"); +} + +BigMapMapsManager::~BigMapMapsManager() = default; + +void BigMapMapsManager::on_initialize() +{ + auto node = get_node(); + const std::string & name = get_plugin_name(); + auto param = [&](const std::string & key, auto value) { + easynav::declare_parameter_if_absent(*node, name + "." + key, value); + node->get_parameter(name + "." + key, value); + return value; + }; + + // The map + const std::string package = param("package", std::string()); + std::string path = param("navmap_path_file", std::string()); + if (path.empty()) { + throw std::runtime_error("[" + name + "] navmap_path_file is not set"); + } + if (!package.empty()) { + path = (std::filesystem::path(easynav::get_package_share_path(package)) / path).string(); + } + ::navmap::NavMap source; + std::error_code ec; + if (!navmap_ros::io::load_from_file(path, source, &ec)) { + throw std::runtime_error("[" + name + "] cannot read " + path + ": " + ec.message()); + } + + // Its levels of detail + LodParams lp; + lp.cell_min = param("cell_size.min", lp.cell_min); + lp.cell_max = param("cell_size.max", lp.cell_max); + lp.min_radius = param("min_radius", lp.min_radius); + lp.max_radius = param("max_radius", lp.max_radius); + forget_distance_ = param("forget_distance", -1.0); + if (forget_distance_ <= 0.0) {forget_distance_ = lp.max_radius;} + min_height_ = param("obstacle_min_height", min_height_); + min_height_per_m_ = param("obstacle_min_height_per_meter", min_height_per_m_); + max_height_ = param("obstacle_max_height", max_height_); + sensor_range_ = param("sensor_range", -1.0); + if (sensor_range_ <= 0.0) {sensor_range_ = lp.min_radius;} + sensor_range_ = std::min(sensor_range_, forget_distance_); + rebuild_period_ = param("rebuild_period", rebuild_period_); + // The memory of sensed obstacles, as the NavMap maps manager's obstacle filter + MemoryParams mp; + mp.clearing = param("memory.clearing", mp.clearing); + mp.decay_time = std::max(0.0, param("memory.decay_time", mp.decay_time)); + mp.p_hit = param("memory.p_hit", mp.p_hit); + mp.p_miss = param("memory.p_miss", mp.p_miss); + if (!(mp.p_hit > 0.5 && mp.p_hit < 1.0) || !(mp.p_miss > 0.0 && mp.p_miss < 0.5)) { + RCLCPP_WARN( + node->get_logger(), "[%s] memory.p_hit must be in (0.5, 1) and memory.p_miss in (0, 0.5): " + "0.7 and 0.4", name.c_str()); + mp.p_hit = 0.7; + mp.p_miss = 0.4; + } + + // Built once, on every core: a startup step, not part of the cycle + const std::size_t source_navcels = source.navcels.size(); + const auto st = lod_.build( + std::move(source), lp, std::max(1u, std::thread::hardware_concurrency())); +#if defined(__GLIBC__) + // the building threads leave most of what they used free but kept by the allocator + malloc_trim(0); +#endif + std::string levels; + for (int k = 0; k < lod_.levels(); ++k) { + char buf[96]; + std::snprintf( + buf, sizeof(buf), "%s%.2f m from %.0f m: %zu NavCels", k ? "; " : "", lod_.cell(k), + lod_.radius(k), st.navcels[k]); + levels += buf; + } + RCLCPP_INFO( + node->get_logger(), "[%s] %s: %zu NavCels, %zu tiles, in %.1f s. Levels: %s", name.c_str(), + path.c_str(), source_navcels, st.tiles, st.seconds, levels.c_str()); + // As fine as the points are thinned: every NavCel an obstacle covers gets one + sensed_ = SensedObstacles(lod_.cell(0) / 2.0, mp); + known_ = static_cast(lod_.cell(0)); + + // Filters, as in the NavMap maps manager + std::vector filters; + filters = param("filters", filters); + for (const auto & f : filters) { + const std::string plugin = param(f + ".plugin", std::string()); + auto instance = filters_loader_->createSharedInstance(plugin); + instance->initialize(node, name + "." + f); + filters_.push_back(instance); + RCLCPP_INFO(node->get_logger(), "[%s] filter %s [%s]", name.c_str(), f.c_str(), plugin.c_str()); + // The detail around obstacles reaches as far as an inflation does + const std::string radius = name + "." + f + ".inflation_radius"; + if (node->has_parameter(radius)) { + margin_ = std::max(margin_, node->get_parameter(radius).as_double()); + } + } + + const std::string base = node->get_node_base_interface()->get_fully_qualified_name() + + std::string("/") + name; + map_pub_ = node->create_publisher( + base + "/map", rclcpp::QoS(1).transient_local().reliable()); + layer_updates_pub_ = node->create_publisher( + base + "/map_updates", rclcpp::QoS(100)); + markers_pub_ = node->create_publisher( + base + "/markers", rclcpp::QoS(1).transient_local().reliable()); + stats_pub_ = node->create_publisher( + base + "/stats", rclcpp::QoS(10)); +} + +bool BigMapMapsManager::robot_pose( + const ::easynav::NavState & nav_state, Eigen::Vector3d & p, double & yaw) const +{ + if (!nav_state.has("robot_pose")) {return false;} + const auto & pose = nav_state.get("robot_pose").pose.pose; + p = {pose.position.x, pose.position.y, pose.position.z}; + const auto & q = pose.orientation; + yaw = std::atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); + return p.allFinite() && std::isfinite(yaw); +} + +bool BigMapMapsManager::sense( + const ::easynav::NavState & nav_state, const Eigen::Vector3d & robot, double yaw) +{ + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty() || !has_map_) {return false;} + pcl::PointCloud cloud; + try { + // In the map frame, each perception with the robot's pose when it was taken (TF at its + // stamp, exact_time): with the latest pose, a cloud taken while the robot turns lands rotated + // (a cloud may be up to the sensors' forget_time old, and is used in every cycle until the + // next one arrives), and walls seen a fraction of a second ago fall on free ground as + // obstacles. Cut to the box around the robot first (heights from its ground, the TF being + // flat), then thinned. + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + const double r = sensor_range_; + PointPerceptionsOpsView view(perceptions); + view.fuse(tf_info.map_frame, true); + view.filter( + {robot.x() - r, robot.y() - r, robot.z() + min_height_ - 1.0}, + {robot.x() + r, robot.y() + r, robot.z() + max_height_ + 1.0}, false); + view.downsample(lod_.cell(0) / 2.0); + cloud = view.as_points(); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 5000, "[%s] perceptions: %s", + get_plugin_name().c_str(), e.what()); + return false; + } + // The ground under the robot, on the map, and its tilt: the robot stands on it, so on a ramp the + // sensor is tilted like the ramp. With a flat TF (2D odometry) the ramp ahead comes at the + // robot's own height (going down a ramp, it would otherwise be taken for an obstacle above the + // ramp's lower part): each point is turned about the robot by what TF lacks of that tilt + // (nothing, if the robot's pose in TF has it already, e.g. a 3D localization) + std::size_t s; + ::navmap::NavCelId cid; + Eigen::Vector3f bary, hit; + Eigen::Vector3f ground( + static_cast(robot.x()), static_cast(robot.y()), static_cast(robot.z())); + Eigen::Quaternionf tilt = Eigen::Quaternionf::Identity(); + if (map_.locate_navcel(robot.cast(), s, cid, bary, &hit)) { + ground.z() = hit.z(); + const auto & v = map_.navcels[cid].v; + const Eigen::Vector3f a = map_.positions.at(v[0]), b = map_.positions.at(v[1]), + d = map_.positions.at(v[2]); + Eigen::Vector3f n = (b - a).cross(d - a); + if (n.norm() > 1e-9f) { + n.normalize(); + if (n.z() < 0.0f) {n = -n;} + Eigen::Vector3f up = Eigen::Vector3f::UnitZ(); // the robot's up in TF + try { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + const auto t = RTTFBuffer::getInstance()->lookupTransform( + tf_info.map_frame, tf_info.robot_footprint_frame, tf2::TimePointZero); + const auto & r = t.transform.rotation; + up = Eigen::Quaternionf( + static_cast(r.w), static_cast(r.x), static_cast(r.y), + static_cast(r.z)) * Eigen::Vector3f::UnitZ(); + } catch (const std::exception &) { + } + tilt = Eigen::Quaternionf::FromTwoVectors(up, n); + } + } + (void)yaw; // the heading comes with each perception's TF + const Eigen::Vector3f at = robot.cast(); + // The evidence of this cycle: the obstacles (on the ground under them), and a ray from the + // robot to every point on the ground or on an obstacle (what it crosses is free) + std::vector hits; + std::vector rays; + rays.reserve(cloud.points.size()); + for (const auto & pt : cloud.points) { + const Eigen::Vector3f o = Eigen::Vector3f(pt.x, pt.y, pt.z) - at; // from the robot + const Eigen::Vector3f q = ground + tilt * o; + // An obstacle is above the map's surface: over no surface (a hole, a building) it is + // the map's own + if (!map_.locate_navcel(q, s, cid, bary, &hit)) {continue;} + // the farther, the higher a point must be: a slight tilt of the robot or the sensor puts the + // far ground above it + const float h = q.z() - hit.z(); + const double range = std::hypot(o.x(), o.y()); + if (h > max_height_) {continue;} // over the robot: neither an obstacle nor free ground + if (h < min_height_ + min_height_per_m_ * range) { + rays.push_back({hit, true}); // the ground: free up to it, and it too + continue; + } + rays.push_back({hit, false}); + // Next to a place with no ground (within cell_min: what the map and the localization are + // sure of) it is the map's own obstacle, seen from the side + bool known = false; + for (int k = 0; k < 8 && !known; ++k) { + const float a = static_cast(k * M_PI / 4.0); + const Eigen::Vector3f n(hit.x() + known_ * std::cos(a), hit.y() + known_ * std::sin(a), + hit.z() + 0.2f); + Eigen::Vector3f nb, nh; + known = !map_.locate_navcel(n, s, cid, nb, &nh); + } + if (known) {continue;} + hits.push_back(hit); + } + return sensed_.update(hits, ground, rays, get_node()->now().seconds()); +} + +void BigMapMapsManager::rebuild( + const std::vector & tiles, const Eigen::Vector3d & robot) +{ + const auto t0 = std::chrono::steady_clock::now(); + map_ = lod_.assemble(tiles, false); // detail() builds the accelerations + const auto t1 = std::chrono::steady_clock::now(); + lod_.detail(map_, robot.x(), robot.y(), sensed_.points(), margin_); + const auto t2 = std::chrono::steady_clock::now(); + RCLCPP_INFO_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 10000, + "[%s] map around (%.1f, %.1f): %zu NavCels from %zu tiles, %zu sensed obstacles; " + "assembled in %.0f ms, detailed in %.0f ms", get_plugin_name().c_str(), robot.x(), robot.y(), + map_.navcels.size(), tiles.size(), sensed_.size(), + std::chrono::duration(t1 - t0).count(), + std::chrono::duration(t2 - t1).count()); + stats_[0] += 1.0; + stats_[1] = select_ms_; + stats_[2] = std::chrono::duration(t1 - t0).count(); + stats_[3] = std::chrono::duration(t2 - t1).count(); + stats_[4] = static_cast(map_.navcels.size()); + tiles_ = tiles; + built_at_ = robot.head<2>(); + built_z_ = robot.z(); + has_map_ = true; + map_changed_ = true; + publish_pending_ = true; +} + +void BigMapMapsManager::publish(const ::navmap::NavMap & nm) +{ + std_msgs::msg::Header header; + header.frame_id = RTTFBuffer::getInstance()->get_tf_info().map_frame; + header.stamp = get_node()->now(); + auto msg = navmap_ros::to_msg(nm); + msg.header = header; + map_pub_->publish(msg); + + // Markers: a circle where each level starts (with its cell size) and the tiles in use, by level + visualization_msgs::msg::MarkerArray ma; + visualization_msgs::msg::Marker clear; + clear.header = header; + clear.action = visualization_msgs::msg::Marker::DELETEALL; + ma.markers.push_back(clear); + const int levels = lod_.levels(); + auto color = [levels](int k, visualization_msgs::msg::Marker & m) { + const float t = levels > 1 ? static_cast(k) / (levels - 1) : 0.0f; // fine: red + m.color.r = 1.0f - t; + m.color.g = 0.4f + 0.4f * t; + m.color.b = t; + m.color.a = 1.0f; + }; + const double z = std::isfinite(built_z_) ? built_z_ + 0.2 : 0.2; + int id = 0; + for (int k = 1; k < levels; ++k) { + visualization_msgs::msg::Marker c; + c.header = header; + c.ns = "level radii"; + c.id = id++; + c.type = visualization_msgs::msg::Marker::LINE_STRIP; + c.scale.x = 0.3; + c.pose.orientation.w = 1.0; + color(k, c); + const double r = lod_.radius(k); + for (int i = 0; i <= 120; ++i) { + geometry_msgs::msg::Point p; + p.x = built_at_.x() + r * std::cos(i * 2.0 * M_PI / 120.0); + p.y = built_at_.y() + r * std::sin(i * 2.0 * M_PI / 120.0); + p.z = z; + c.points.push_back(p); + } + ma.markers.push_back(c); + visualization_msgs::msg::Marker t; + t.header = header; + t.ns = "level radii"; + t.id = id++; + t.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + t.pose.position.x = built_at_.x(); + t.pose.position.y = built_at_.y() + r; + t.pose.position.z = z + 1.0; + t.pose.orientation.w = 1.0; + t.scale.z = 2.0; + color(k, t); + char buf[48]; + std::snprintf(buf, sizeof(buf), "%.1f m cells from %.0f m", lod_.cell(k), r); + t.text = buf; + ma.markers.push_back(t); + } + std::vector tiles(levels); + for (int k = 0; k < levels; ++k) { + auto & m = tiles[k]; + m.header = header; + m.ns = "tiles"; + m.id = id++; + m.type = visualization_msgs::msg::Marker::LINE_LIST; + m.scale.x = 0.1; + m.pose.orientation.w = 1.0; + color(k, m); + m.color.a = 0.6f; + } + for (const auto t : tiles_) { + const auto b = lod_.bounds(t); + auto & m = tiles[lod_.level(t)]; + const double xs[4] = {b[0], b[1], b[1], b[0]}, ys[4] = {b[2], b[2], b[3], b[3]}; + for (int i = 0; i < 4; ++i) { + geometry_msgs::msg::Point a, c; + a.x = xs[i]; + a.y = ys[i]; + c.x = xs[(i + 1) % 4]; + c.y = ys[(i + 1) % 4]; + a.z = c.z = z; + m.points.push_back(a); + m.points.push_back(c); + } + } + for (auto & m : tiles) { + if (!m.points.empty()) {ma.markers.push_back(std::move(m));} + } + markers_pub_->publish(ma); +} + +void BigMapMapsManager::update(::easynav::NavState & nav_state) +{ + if (lod_.empty()) {return;} + const double now = get_node()->now().seconds(); + Eigen::Vector3d robot; + double yaw = 0.0; + const bool located = robot_pose(nav_state, robot, yaw); + if (!located) { + if (has_map_) {robot << built_at_, 0.0;} else {robot.setZero();} + } + + using Ms = std::chrono::duration; + const auto ts = std::chrono::steady_clock::now(); + if (located) { + sensed_changed_ |= sense(nav_state, robot, yaw); + sensed_changed_ |= sensed_.forget_beyond(robot.x(), robot.y(), forget_distance_) > 0; + } + + // The map again when the tiles change, the robot has gone a few metres (the detail moves with + // it) or the sensed obstacles change; not more often than rebuild_period_ + stats_[5] = Ms(std::chrono::steady_clock::now() - ts).count(); + const auto tsel = std::chrono::steady_clock::now(); + const auto tiles = lod_.select(robot.x(), robot.y()); + select_ms_ = Ms(std::chrono::steady_clock::now() - tsel).count(); + const bool moved = (robot.head<2>() - built_at_).norm() > lod_.params().min_radius / 4.0; + if (!has_map_ || + ((tiles != tiles_ || moved || sensed_changed_) && now - last_build_ >= rebuild_period_)) + { + rebuild(tiles, robot); + sensed_changed_ = false; + last_build_ = now; + } + if (map_changed_) { + nav_state.set("map.navmap", map_); + map_changed_ = false; + } + // Filters update the map in NavState in place, and record which cells they change + const std::size_t subscribers = layer_updates_pub_->get_subscription_count(); + const bool new_subscriber = subscribers > last_subscribers_; + last_subscribers_ = subscribers; + std_msgs::msg::Header header; + header.frame_id = RTTFBuffer::getInstance()->get_tf_info().map_frame; + header.stamp = get_node()->now(); + const auto tf = std::chrono::steady_clock::now(); + for (const auto & filter : filters_) { + filter->set_map_resolution(static_cast(lod_.cell(0))); + filter->update(nav_state); + // A map not published yet carries its layers itself: an update now would be of a geometry + // the subscribers do not have yet + if (subscribers == 0 || publish_pending_ || !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)); + } + stats_[6] = Ms(std::chrono::steady_clock::now() - tf).count(); + nav_state.set("map.bigmap_stats", stats_); + if (stats_pub_->get_subscription_count() > 0) { + // [time (s), rebuilds, select, assemble, detail (ms), NavCels, sensing, filters (ms), + // sensed obstacles] + std_msgs::msg::Float64MultiArray m; + m.data.reserve(stats_.size() + 2); + m.data.push_back(now); + m.data.insert(m.data.end(), stats_.begin(), stats_.end()); + m.data.push_back(static_cast(sensed_.size())); + stats_pub_->publish(m); + } + if (nav_state.has("map.navmap")) { + const auto & nm = nav_state.get<::navmap::NavMap>("map.navmap"); + // the map as the filters left it (with their layers, e.g. inflated_obstacles) + if (publish_pending_ && now - last_publish_ >= publish_period_) { + publish(nm); + publish_pending_ = false; + last_publish_ = now; + } + for (const auto & l : nm.list_layers()) { + nm.layers.get(l)->clear_changes(); + } + } +} + +} // namespace bigmap +} // namespace easynav + +PLUGINLIB_EXPORT_CLASS(easynav::bigmap::BigMapMapsManager, easynav::MapsManagerBase) 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/tiled_lod.cpp b/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp new file mode 100644 index 00000000..34184ebf --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp @@ -0,0 +1,843 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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/tiled_lod.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/Costs.hpp" +#include "navmap_core/Simplify.hpp" +#include "navmap_core/Subdivide.hpp" + +namespace easynav +{ +namespace bigmap +{ + +namespace +{ + +using ::navmap::NavCel; +using ::navmap::NavCelId; +using ::navmap::NavMap; +using ::navmap::PointId; + +// Calls f(view) with the typed view of @p base (no-op for an unknown type). +template +void with_view(const std::shared_ptr<::navmap::LayerViewBase> & base, F && f) +{ + using ::navmap::LayerView; + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} +} + +// Runs fn(i) for i in [0, n) on up to `threads` threads +void parallel_for(std::size_t n, unsigned threads, const std::function & fn) +{ + threads = std::max(1u, std::min(threads, static_cast(n))); + if (threads <= 1) { + for (std::size_t i = 0; i < n; ++i) { + fn(i); + } + return; + } + std::atomic next{0}; + std::vector pool; + for (unsigned t = 0; t < threads; ++t) { + pool.emplace_back( + [&]() { + for (std::size_t i = next++; i < n; i = next++) { + fn(i); + } + }); + } + for (auto & th : pool) { + th.join(); + } +} + +// The surface of each NavCel +std::vector surface_of(const NavMap & nm) +{ + std::vector s(nm.navcels.size(), 0); + for (std::size_t k = 0; k < nm.surfaces.size(); ++k) { + for (const NavCelId c : nm.surfaces[k].navcels) { + if (c < s.size()) {s[c] = k;} + } + } + return s; +} + +// A NavMap of some NavCels of `src` (vertices compacted) with their layers, surfaces, colours +NavMap subset( + const NavMap & src, const std::vector & cels, const std::vector & surf) +{ + NavMap out; + for (const auto & s : src.surfaces) { + out.create_surface(s.frame_id); + } + const bool colored = src.colors && src.colors->r.size() == src.positions.size(); + if (colored) {out.colors.emplace();} + std::unordered_map vmap; + vmap.reserve(cels.size() * 2); + for (const NavCelId c : cels) { + NavCel cel; + for (int k = 0; k < 3; ++k) { + const PointId v = src.navcels[c].v[k]; + auto [it, fresh] = vmap.try_emplace(v, static_cast(out.positions.size())); + if (fresh) { + out.positions.x.push_back(src.positions.x[v]); + out.positions.y.push_back(src.positions.y[v]); + out.positions.z.push_back(src.positions.z[v]); + if (colored) { + out.colors->r.push_back(src.colors->r[v]); + out.colors->g.push_back(src.colors->g[v]); + out.colors->b.push_back(src.colors->b[v]); + out.colors->a.push_back(src.colors->a[v]); + } + } + cel.v[k] = it->second; + } + out.surfaces[surf[c]].navcels.push_back(static_cast(out.navcels.size())); + out.navcels.push_back(cel); + } + for (const auto & name : src.list_layers()) { + with_view( + src.layers.get(name), [&](const auto & v) { + using T = std::decay_t; + if (v.size() != src.navcels.size()) {return;} + auto dst = out.add_layer(name); + for (std::size_t i = 0; i < cels.size(); ++i) { + dst->data()[i] = v.data()[cels[i]]; + } + }); + } + out.layer_meta = src.layer_meta; + return out; +} + +// What a tile keeps of a NavMap: its geometry, surfaces, layers and colours. Not the +// accelerations (BVH, adjacency, locate grids), which only the assembled map needs: with tens of +// thousands of tiles they would be most of the memory +NavMap slim(NavMap && m) +{ + NavMap out; + out.positions = std::move(m.positions); + out.positions.x.shrink_to_fit(); + out.positions.y.shrink_to_fit(); + out.positions.z.shrink_to_fit(); + out.colors = std::move(m.colors); + out.navcels.resize(m.navcels.size()); + for (std::size_t i = 0; i < m.navcels.size(); ++i) { + for (int k = 0; k < 3; ++k) { + out.navcels[i].v[k] = m.navcels[i].v[k]; + } + } + for (auto & s : m.surfaces) { + out.create_surface(s.frame_id); + out.surfaces.back().navcels = std::move(s.navcels); + } + out.layers = std::move(m.layers); + out.layer_meta = std::move(m.layer_meta); + return out; +} + +// Exact position of a vertex, as a hash key +struct Key +{ + float x, y, z; + bool operator==(const Key & o) const {return x == o.x && y == o.y && z == o.z;} +}; +struct KeyHash +{ + std::size_t operator()(const Key & k) const + { + std::uint32_t a, b, c; + std::memcpy(&a, &k.x, 4); + std::memcpy(&b, &k.y, 4); + std::memcpy(&c, &k.z, 4); + return (static_cast(a) * 0x9E3779B97F4A7C15ull) ^ + (static_cast(b) * 0xC2B2AE3D27D4EB4Full) ^ (c * 0x165667B19E3779F9ull); + } +}; + +int line_index(const std::vector & lines, float v) +{ + auto it = std::lower_bound(lines.begin(), lines.end(), v); + return it != lines.end() && *it == v ? static_cast(it - lines.begin()) : -1; +} + +} // namespace + +double TiledLod::cell(int level) const +{ + return std::min(p_.cell_min * std::pow(2.0, level), p_.cell_max); +} + +double TiledLod::radius(int level) const +{ + if (level <= 0) {return 0.0;} + if (top_ <= 1) {return p_.min_radius;} + return p_.min_radius * + std::pow(p_.max_radius / p_.min_radius, static_cast(level - 1) / (top_ - 1)); +} + +int TiledLod::level_at(double d) const +{ + int k = 0; + while (k < top_ && d >= radius(k + 1)) {++k;} + return k; +} + +TiledLod::Stats TiledLod::build(NavMap map, const LodParams & params, unsigned threads) +{ + const auto t0 = std::chrono::steady_clock::now(); + p_ = params; + p_.cell_min = std::max(1e-3, p_.cell_min); + p_.cell_max = std::max(p_.cell_min, p_.cell_max); + p_.min_radius = std::max(0.0, p_.min_radius); + p_.max_radius = std::max(p_.min_radius, p_.max_radius); + const double tile = p_.tile > 0.0 ? p_.tile : std::max(p_.min_radius / 2.0, 4.0 * p_.cell_min); + p_.tile = tile; + top_ = p_.cell_max > p_.cell_min ? + static_cast(std::ceil(std::log2(p_.cell_max / p_.cell_min) - 1e-9)) : 0; + nodes_.clear(); + roots_.clear(); + xs_.clear(); + ys_.clear(); + Stats st; + st.navcels.assign(top_ + 1, 0); + if (map.navcels.empty()) {return st;} + + // The grid + float lo[2] = {std::numeric_limits::infinity(), std::numeric_limits::infinity()}; + float hi[2] = {-lo[0], -lo[1]}; + for (std::size_t i = 0; i < map.positions.size(); ++i) { + lo[0] = std::min(lo[0], map.positions.x[i]); + hi[0] = std::max(hi[0], map.positions.x[i]); + lo[1] = std::min(lo[1], map.positions.y[i]); + hi[1] = std::max(hi[1], map.positions.y[i]); + } + int n[2]; + double origin[2]; + for (int a = 0; a < 2; ++a) { + origin[a] = std::floor(lo[a] / tile) * tile; + n[a] = std::max(1, static_cast(std::floor((hi[a] - origin[a]) / tile)) + 1); + auto & lines = a == 0 ? xs_ : ys_; + for (int i = 0; i <= n[a]; ++i) { + lines.push_back(static_cast(origin[a] + i * tile)); + } + } + NavMap cut = std::move(map); // cut in place + ::navmap::cut_along_lines( + cut, {{xs_.begin() + 1, xs_.end() - 1}, {ys_.begin() + 1, ys_.end() - 1}}); + + // The finest tiles: the NavCels whose centroid is in each square + std::map, std::vector> squares; + for (NavCelId c = 0; c < cut.navcels.size(); ++c) { + const auto q = cut.navcel_centroid(c); + const int i = std::clamp(static_cast(std::floor((q.x() - origin[0]) / tile)), 0, n[0] - 1); + const int j = std::clamp(static_cast(std::floor((q.y() - origin[1]) / tile)), 0, n[1] - 1); + squares[{i, j}].push_back(c); + } + const auto surf = surface_of(cut); + std::vector> cels_of; + for (auto & [ij, cels] : squares) { + Node node; + node.level = 0; + node.lines = {ij.first, ij.first + 1, ij.second, ij.second + 1}; + nodes_.push_back(std::move(node)); + cels_of.push_back(std::move(cels)); + } + squares.clear(); + st.tiles = nodes_.size(); + // each tile taken from the cut map just before it is simplified: the unsimplified tiles are + // never all in memory at once + parallel_for( + nodes_.size(), threads, [this, &cut, &cels_of, &surf](std::size_t i) { + nodes_[i].mesh = subset(cut, cels_of[i], surf); + cels_of[i] = {}; + simplify_node(nodes_[i]); + }); + cut = NavMap(); + + // Each level from the one below: four tiles put together, simplified with twice the error + std::size_t level_begin = 0; + for (int k = 1; k <= top_; ++k) { + const std::size_t level_end = nodes_.size(); + std::map, std::vector> parents; + for (std::size_t i = level_begin; i < level_end; ++i) { + const auto & c = nodes_[i]; + parents[{c.lines[0] >> k, c.lines[2] >> k}].push_back(static_cast(i)); + } + const std::size_t first = nodes_.size(); + for (const auto & [ij, kids] : parents) { + Node node; + node.level = k; + node.lines = {ij.first << k, std::min((ij.first + 1) << k, n[0]), + ij.second << k, std::min((ij.second + 1) << k, n[1])}; + for (const auto kid : kids) { + const auto & c = nodes_[kid]; + const int q = ((c.lines[0] >> (k - 1)) & 1) + 2 * ((c.lines[2] >> (k - 1)) & 1); + node.child[q] = static_cast(kid); + } + nodes_.push_back(std::move(node)); + } + parallel_for( + nodes_.size() - first, threads, [this, first](std::size_t i) { + auto & node = nodes_[first + i]; + std::vector kids; + for (const int c : node.child) { + if (c >= 0) {kids.push_back(static_cast(c));} + } + node.mesh = stitch(kids, false); + simplify_node(node); + }); + level_begin = level_end; + } + for (std::size_t i = 0; i < nodes_.size(); ++i) { + st.navcels[nodes_[i].level] += nodes_[i].mesh.navcels.size(); + if (nodes_[i].level == top_) {roots_.push_back(static_cast(i));} + } + st.seconds = std::chrono::duration(std::chrono::steady_clock::now() - t0).count(); + return st; +} + +std::vector TiledLod::border_locks(const Node & n) const +{ + const auto & m = n.mesh; + const float x0 = xs_[n.lines[0]], x1 = xs_[n.lines[1]]; + const float y0 = ys_[n.lines[2]], y1 = ys_[n.lines[3]]; + auto sides = [&](PointId v) { + const float x = m.positions.x[v], y = m.positions.y[v]; + return (x == x0 ? 1 : 0) | (x == x1 ? 2 : 0) | (y == y0 ? 4 : 0) | (y == y1 ? 8 : 0); + }; + std::vector lock(m.positions.size(), 0); + for (PointId v = 0; v < m.positions.size(); ++v) { + const int s = sides(v); + if (s && (s & (s - 1))) {lock[v] = 1;} // a corner + } + // Where the map's border leaves a side of the tile + std::unordered_map edges; + auto key = [](PointId a, PointId b) { + return a < b ? (static_cast(a) << 32) | b : + (static_cast(b) << 32) | a; + }; + for (const auto & c : m.navcels) { + for (int k = 0; k < 3; ++k) { + edges[key(c.v[k], c.v[(k + 1) % 3])]++; + } + } + for (const auto & [e, count] : edges) { + if (count != 1) {continue;} + const PointId a = static_cast(e >> 32), b = static_cast(e); + const int sa = sides(a), sb = sides(b); + if (sa && !(sa & sb)) {lock[a] = 1;} + if (sb && !(sa & sb)) {lock[b] = 1;} + } + std::vector out; + for (PointId v = 0; v < lock.size(); ++v) { + if (lock[v]) {out.push_back(v);} + } + return out; +} + +void TiledLod::simplify_node(Node & n) const +{ + ::navmap::SimplifyParams sp; + sp.max_error = static_cast(cell(n.level) / 2.0); + sp.max_edge_length = static_cast(p_.cell_max); + sp.keep_vertices = border_locks(n); + ::navmap::simplify(n.mesh, sp); + n.mesh = slim(std::move(n.mesh)); +} + +double TiledLod::distance(const Node & n, double x, double y) const +{ + const double dx = std::max({static_cast(xs_[n.lines[0]]) - x, 0.0, + x - static_cast(xs_[n.lines[1]])}); + const double dy = std::max({static_cast(ys_[n.lines[2]]) - y, 0.0, + y - static_cast(ys_[n.lines[3]])}); + return std::hypot(dx, dy); +} + +std::vector TiledLod::select(double x, double y) const +{ + std::vector out, stack(roots_.rbegin(), roots_.rend()); + while (!stack.empty()) { + const std::uint32_t i = stack.back(); + stack.pop_back(); + const auto & n = nodes_[i]; + if (n.level == 0 || distance(n, x, y) >= radius(n.level)) { + out.push_back(i); + continue; + } + for (int q = 3; q >= 0; --q) { + if (n.child[q] >= 0) {stack.push_back(static_cast(n.child[q]));} + } + } + return out; +} + +NavMap TiledLod::assemble(const std::vector & tiles, bool accels) const +{ + return stitch(tiles, accels); +} + +void TiledLod::detail( + NavMap & nm, double x, double y, const std::vector & obstacles, + double margin) const +{ + if (nm.navcels.empty()) {return;} + const float fine = static_cast(p_.cell_min); + auto near_robot = [&](const Eigen::Vector2f & q, double extra) { + return std::hypot(q.x() - x, q.y() - y) <= p_.min_radius + extra; + }; + + // Seeds near the robot: the borders of the map (points every cell_min along them) and the + // sensed obstacles. Farther obstacles keep the cell size of their place. + std::unordered_map edges; + auto key = [](PointId a, PointId b) { + return a < b ? (static_cast(a) << 32) | b : + (static_cast(b) << 32) | a; + }; + const float cell_max = static_cast(p_.cell_max); + for (const auto & c : nm.navcels) { + // only near the robot: a NavCel left out has all its vertices farther than min_radius plus + // cell_max (no side is longer), so no edge it shares within min_radius looks open + if (!near_robot(nm.positions.at(c.v[0]).head<2>(), cell_max) && + !near_robot(nm.positions.at(c.v[1]).head<2>(), cell_max) && + !near_robot(nm.positions.at(c.v[2]).head<2>(), cell_max)) + { + continue; + } + for (int k = 0; k < 3; ++k) { + edges[key(c.v[k], c.v[(k + 1) % 3])]++; + } + } + std::vector seeds; + for (const auto & [e, use] : edges) { + if (use != 1) {continue;} + const Eigen::Vector2f a = nm.positions.at(static_cast(e >> 32)).head<2>(); + const Eigen::Vector2f b = nm.positions.at(static_cast(e & 0xffffffffu)).head<2>(); + const int n = std::max(1, static_cast(std::ceil((b - a).norm() / fine))); + for (int i = 0; i <= n; ++i) { + const Eigen::Vector2f q = a + (b - a) * (static_cast(i) / n); + if (near_robot(q, 0.0)) {seeds.push_back(q);} + } + } + struct Far + { + Eigen::Vector2f p; + float size; + }; + std::vector far; + for (const auto & q : obstacles) { + if (near_robot(q.head<2>(), 0.0)) { + seeds.push_back(q.head<2>()); + } else { + far.push_back( + {q.head<2>(), static_cast(cell(level_at(std::hypot(q.x() - x, q.y() - y))))}); + } + } + + // Every NavCel within the margin (the inflation) of a seed fine: the cost of a NavCel is that + // of its centroid, so a large one next to an obstacle would read as free. A NavCel holding an + // obstacle farther away, as fine as the cell size of its place. One refinement for all. + const double band = std::max(margin, p_.cell_min); + auto grid_key = [](std::int64_t i, std::int64_t j) {return (i << 32) ^ (j & 0xffffffff);}; + auto cell_of = [](float v, double cell) {return static_cast(std::floor(v / cell));}; + std::unordered_map> seed_grid; + for (const auto & q : seeds) { + seed_grid[grid_key(cell_of(q.x(), band), cell_of(q.y(), band))].push_back(q); + } + const double far_cell = p_.cell_max; + std::unordered_map> far_grid; + for (const auto & f : far) { + far_grid[grid_key(cell_of(f.p.x(), far_cell), cell_of(f.p.y(), far_cell))].push_back(f); + } + auto cross = [](const Eigen::Vector2f & u, const Eigen::Vector2f & v) { + return u.x() * v.y() - u.y() * v.x(); + }; + auto size_at = [&]( + const Eigen::Vector3f & a3, const Eigen::Vector3f & b3, const Eigen::Vector3f & c3) { + const Eigen::Vector2f a = a3.head<2>(), b = b3.head<2>(), c = c3.head<2>(); + const Eigen::Vector2f q = (a + b + c) / 3.0f; + const float reach = std::max({(a - q).norm(), (b - q).norm(), (c - q).norm()}); + if (!seed_grid.empty() && near_robot(q, band + reach)) { + const double r = band + reach; + const int span = static_cast(std::ceil(r / band)); + const auto ci = cell_of(q.x(), band), cj = cell_of(q.y(), band); + for (std::int64_t i = ci - span; i <= ci + span; ++i) { + for (std::int64_t j = cj - span; j <= cj + span; ++j) { + const auto it = seed_grid.find(grid_key(i, j)); + if (it == seed_grid.end()) {continue;} + for (const auto & sd : it->second) { + if ((sd - q).norm() < r) {return fine;} + } + } + } + } + float out = std::numeric_limits::infinity(); + if (far_grid.empty()) {return out;} + const Eigen::Vector2f lo = a.cwiseMin(b).cwiseMin(c), hi = a.cwiseMax(b).cwiseMax(c); + const float area = cross(b - a, c - a); + const float eps = 1e-6f * std::abs(area) + 1e-9f; + for (auto i = cell_of(lo.x(), far_cell); i <= cell_of(hi.x(), far_cell); ++i) { + for (auto j = cell_of(lo.y(), far_cell); j <= cell_of(hi.y(), far_cell); ++j) { + const auto it = far_grid.find(grid_key(i, j)); + if (it == far_grid.end()) {continue;} + for (const auto & f : it->second) { + const float s1 = cross(b - a, f.p - a), s2 = cross(c - b, f.p - b), + s3 = cross(a - c, f.p - c); + const bool in = area > 0.0f ? (s1 >= -eps && s2 >= -eps && s3 >= -eps) : + (s1 <= eps && s2 <= eps && s3 <= eps); + if (in) {out = std::min(out, f.size);} + } + } + } + return out; + }; + const auto st = ::navmap::refine(nm, size_at); + if (st.navcels_after == st.navcels_before) {nm.rebuild_geometry_accels();} // refine did not + + auto layer = nm.add_layer( + "obstacles", "obstacles: the map's and the sensed ones", "", ::navmap::FREE_SPACE); + for (const auto & q : obstacles) { + std::size_t s; + NavCelId c; + Eigen::Vector3f bary; + if (nm.locate_navcel(q, s, c, bary, nullptr)) { + layer->data()[c] = ::navmap::LETHAL_OBSTACLE; + layer->mark_changed(c); + } + } +} + +NavMap TiledLod::stitch(const std::vector & nodes, bool accels) const +{ + // Vertices: those on a grid line are shared by the tiles on both sides (same exact position) + std::vector P; + std::vector> C; + bool colored = !nodes.empty(); + std::size_t surfaces = 0; + for (const auto i : nodes) { + const auto & m = nodes_[i].mesh; + colored &= m.colors && m.colors->r.size() == m.positions.size(); + surfaces = std::max(surfaces, m.surfaces.size()); + } + std::unordered_map shared; + struct OnLine + { + float s; // coordinate along the line + std::uint32_t v; + }; + std::vector> vlines(xs_.size()), hlines(ys_.size()); + struct Src + { + std::uint32_t node; + NavCelId cel; + }; + std::vector> tris; + std::vector src; + std::vector> surf_of(nodes.size()); + for (std::size_t ni = 0; ni < nodes.size(); ++ni) { + const auto & m = nodes_[nodes[ni]].mesh; + surf_of[ni] = surface_of(m); + std::vector g(m.positions.size()); + for (PointId v = 0; v < m.positions.size(); ++v) { + const Eigen::Vector3f p = m.positions.at(v); + const int ix = line_index(xs_, p.x()), iy = line_index(ys_, p.y()); + auto add = [&]() { + P.push_back(p); + if (colored) { + C.push_back({m.colors->r[v], m.colors->g[v], m.colors->b[v], m.colors->a[v]}); + } + return static_cast(P.size() - 1); + }; + if (ix < 0 && iy < 0) {g[v] = add(); continue;} + auto [it, fresh] = shared.try_emplace(Key{p.x(), p.y(), p.z()}, 0u); + if (fresh) { + it->second = add(); + if (ix >= 0) {vlines[ix].push_back({p.y(), it->second});} + if (iy >= 0) {hlines[iy].push_back({p.x(), it->second});} + } + g[v] = it->second; + } + for (NavCelId c = 0; c < m.navcels.size(); ++c) { + const auto & v = m.navcels[c].v; + tris.push_back({g[v[0]], g[v[1]], g[v[2]]}); + src.push_back({static_cast(ni), c}); + } + } + for (auto * lines : {&vlines, &hlines}) { + for (auto & l : *lines) { + std::sort(l.begin(), l.end(), [](const OnLine & a, const OnLine & b) {return a.s < b.s;}); + } + } + + // Stitching: an edge on a grid line gets the vertices that the tiles on the other side of the + // line have on it, between its ends. All the vertices are the source's own (a simplified side + // only loses vertices along it), and along a line the ground of the source is made of chains of + // edges whose ends (where a border crosses the line) and tile corners are kept at every level. + // Floors over each other have edges on the same stretch of a line: a vertex goes into an edge + // of another chain (a chain whose ends were simplified away on one side) only if it is within + // kSameFloor of it in height, so floors, or a floor that ends at the line above another, are + // never joined; on each side it goes into the covering edge nearest in height. + constexpr float kSameFloor = 0.5f; // m: floors are farther apart than this + struct LineEdge + { + std::uint32_t a, b; // ends + float sa, sb; // their coordinates along the line + const std::vector * line; + std::uint64_t lid; // the line: vertical ones first, then horizontal + int side; // 0 / 1: the side of the line its NavCel is on + }; + std::vector ledges; + std::vector> tri_edge(tris.size(), {-1, -1, -1}); + for (std::size_t t = 0; t < tris.size(); ++t) { + const auto & g = tris[t]; + for (int k = 0; k < 3; ++k) { + const std::uint32_t a = g[k], b = g[(k + 1) % 3]; + const Eigen::Vector3f pa = P[a], pb = P[b], po = P[g[(k + 2) % 3]]; + LineEdge e{a, b, 0.0f, 0.0f, nullptr, 0, 0}; + if (pa.x() == pb.x()) { + const int ix = line_index(xs_, pa.x()); + if (ix >= 0) { + e = {a, b, pa.y(), pb.y(), &vlines[ix], static_cast(ix), + po.x() > pa.x() ? 1 : 0}; + } + } + if (!e.line && pa.y() == pb.y()) { + const int iy = line_index(ys_, pa.y()); + if (iy >= 0) { + e = {a, b, pa.x(), pb.x(), &hlines[iy], xs_.size() + static_cast(iy), + po.y() > pa.y() ? 1 : 0}; + } + } + if (!e.line || e.sa == e.sb) {continue;} + tri_edge[t][k] = static_cast(ledges.size()); + ledges.push_back(e); + } + } + // The chains: the edges on each line, joined at their shared vertices (a vertex at a crossing + // of two lines is on two: keyed by the line too) + std::unordered_map on_line; // (line, vertex) -> index + auto line_vertex = [](std::uint64_t lid, std::uint32_t v) { + return (lid << 32) | static_cast(v); + }; + std::vector chain; + auto index_of = [&](std::uint64_t lid, std::uint32_t v) { + auto [it, fresh] = on_line.try_emplace(line_vertex(lid, v), 0u); + if (fresh) { + it->second = static_cast(chain.size()); + chain.push_back(it->second); + } + return it->second; + }; + std::function find = [&](std::uint32_t x) { + while (chain[x] != x) { + chain[x] = chain[chain[x]]; + x = chain[x]; + } + return x; + }; + for (const auto & e : ledges) { + const auto ia = index_of(e.lid, e.a), ib = index_of(e.lid, e.b); + chain[find(ia)] = find(ib); + } + auto chain_of = [&](std::uint64_t lid, std::uint32_t v) -> std::int64_t { + const auto it = on_line.find(line_vertex(lid, v)); + return it == on_line.end() ? -1 : static_cast(find(it->second)); + }; + // For each vertex and side of a line, the edge of its chain on that side that covers it + struct Best + { + float dz; + int edge; // -1: an edge on that side ends at the vertex, nothing to do + }; + std::unordered_map best; + auto best_key = [](const LineEdge & e, std::uint32_t v) { + return (e.lid << 33) | (static_cast(v) << 1) | + static_cast(e.side); + }; + auto offer = [&](std::uint64_t k, float dz, int edge) { + auto [it, fresh] = best.try_emplace(k, Best{dz, edge}); + if (!fresh && dz < it->second.dz) {it->second = Best{dz, edge};} + }; + for (std::size_t i = 0; i < ledges.size(); ++i) { + const auto & e = ledges[i]; + offer(best_key(e, e.a), 0.0f, -1); + offer(best_key(e, e.b), 0.0f, -1); + const std::int64_t own = chain_of(e.lid, e.a); + const float lo = std::min(e.sa, e.sb), hi = std::max(e.sa, e.sb); + auto it = std::upper_bound( + e.line->begin(), e.line->end(), lo, [](float s, const OnLine & o) {return s < o.s;}); + for (; it != e.line->end() && it->s < hi; ++it) { + const float t = (it->s - e.sa) / (e.sb - e.sa); + const float dz = std::abs(P[it->v].z() - (P[e.a].z() + t * (P[e.b].z() - P[e.a].z()))); + if (dz > kSameFloor && chain_of(e.lid, it->v) != own) {continue;} + offer(best_key(e, it->v), dz, static_cast(i)); + } + } + std::vector> inserted(ledges.size()); + for (std::size_t i = 0; i < ledges.size(); ++i) { + const auto & e = ledges[i]; + const float lo = std::min(e.sa, e.sb), hi = std::max(e.sa, e.sb); + auto it = std::upper_bound( + e.line->begin(), e.line->end(), lo, [](float s, const OnLine & o) {return s < o.s;}); + for (; it != e.line->end() && it->s < hi; ++it) { + const auto b = best.find(best_key(e, it->v)); + if (b != best.end() && b->second.edge == static_cast(i)) {inserted[i].push_back(it->v);} + } + if (e.sa > e.sb) {std::reverse(inserted[i].begin(), inserted[i].end());} // from a to b + } + auto between = [&](std::size_t t, int k, std::vector & out) { + out.clear(); + if (tri_edge[t][k] >= 0) {out = inserted[tri_edge[t][k]];} + }; + + std::vector> out_tris; + std::vector out_src; + out_tris.reserve(tris.size()); + out_src.reserve(tris.size()); + std::array, 3> ins; + for (std::size_t t = 0; t < tris.size(); ++t) { + const auto & g = tris[t]; + int edges_with = 0, which = -1; + for (int k = 0; k < 3; ++k) { + between(t, k, ins[k]); + if (!ins[k].empty()) {++edges_with; which = k;} + } + if (edges_with == 0) { + out_tris.push_back(g); + out_src.push_back(src[t]); + } else if (edges_with == 1) { + // a fan from the opposite vertex, along the edge (same winding) + const std::uint32_t a = g[which], b = g[(which + 1) % 3], o = g[(which + 2) % 3]; + std::uint32_t prev = a; + for (const auto v : ins[which]) { + out_tris.push_back({prev, v, o}); + out_src.push_back(src[t]); + prev = v; + } + out_tris.push_back({prev, b, o}); + out_src.push_back(src[t]); + } else { + // vertices on more than one edge: a fan from the centroid + std::vector poly; + for (int k = 0; k < 3; ++k) { + poly.push_back(g[k]); + poly.insert(poly.end(), ins[k].begin(), ins[k].end()); + } + P.push_back((P[g[0]] + P[g[1]] + P[g[2]]) / 3.0f); + if (colored) { + std::array c; + for (int ch = 0; ch < 4; ++ch) { + c[ch] = static_cast((C[g[0]][ch] + C[g[1]][ch] + C[g[2]][ch]) / 3); + } + C.push_back(c); + } + const std::uint32_t mid = static_cast(P.size() - 1); + for (std::size_t i = 0; i < poly.size(); ++i) { + out_tris.push_back({poly[i], poly[(i + 1) % poly.size()], mid}); + out_src.push_back(src[t]); + } + } + } + + // The NavMap: geometry, surfaces, layers, colours + NavMap out; + for (std::size_t s = 0; s < surfaces; ++s) { + std::string frame; + for (const auto i : nodes) { + if (s < nodes_[i].mesh.surfaces.size()) {frame = nodes_[i].mesh.surfaces[s].frame_id; break;} + } + out.create_surface(frame); + } + out.positions.x.reserve(P.size()); + out.positions.y.reserve(P.size()); + out.positions.z.reserve(P.size()); + for (const auto & p : P) { + out.positions.x.push_back(p.x()); + out.positions.y.push_back(p.y()); + out.positions.z.push_back(p.z()); + } + if (colored) { + out.colors.emplace(); + for (const auto & c : C) { + out.colors->r.push_back(c[0]); + out.colors->g.push_back(c[1]); + out.colors->b.push_back(c[2]); + out.colors->a.push_back(c[3]); + } + } + out.navcels.resize(out_tris.size()); + for (std::size_t i = 0; i < out_tris.size(); ++i) { + for (int k = 0; k < 3; ++k) { + out.navcels[i].v[k] = out_tris[i][k]; + } + const auto & s = out_src[i]; + out.surfaces[surf_of[s.node][s.cel]].navcels.push_back(static_cast(i)); + } + std::map layer_owner; // a tile that has each layer + for (std::size_t ni = 0; ni < nodes.size(); ++ni) { + for (const auto & name : nodes_[nodes[ni]].mesh.list_layers()) { + layer_owner.emplace(name, static_cast(ni)); + } + } + for (const auto & [name, owner] : layer_owner) { + with_view( + nodes_[nodes[owner]].mesh.layers.get(name), [&](const auto & first) { + using T = std::decay_t; + auto dst = out.add_layer(name); + std::vector *> per_node(nodes.size(), nullptr); + for (std::size_t ni = 0; ni < nodes.size(); ++ni) { + const auto & m = nodes_[nodes[ni]].mesh; + auto v = std::dynamic_pointer_cast<::navmap::LayerView>(m.layers.get(name)); + if (v && v->size() == m.navcels.size()) {per_node[ni] = &v->data();} + } + for (std::size_t i = 0; i < out_src.size(); ++i) { + const auto * d = per_node[out_src[i].node]; + if (d) {dst->data()[i] = (*d)[out_src[i].cel];} + } + const auto meta = nodes_[nodes[owner]].mesh.layer_meta.find(name); + if (meta != nodes_[nodes[owner]].mesh.layer_meta.end()) { + out.layer_meta[name] = meta->second; + } + }); + } + if (accels) {out.rebuild_geometry_accels();} + return out; +} + +} // namespace bigmap +} // namespace easynav 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..841b60e6 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp @@ -0,0 +1,399 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 what the sensors see + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_bigmap_maps_manager/BigMapMapsManager.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "navmap_core/Costs.hpp" +#include "navmap_ros/navmap_io.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +using easynav::bigmap::BigMapMapsManager; +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +// 120 x 120 m at 0.5 m: blocks of 14 x 14 m every 20 m, 6 m streets +std::string town_file() +{ + NavMap nm; + const auto s = nm.create_surface("map"); + const float res = 0.5f; + const int n = 240; + for (int j = 0; j <= n; ++j) { + for (int i = 0; i <= n; ++i) { + nm.positions.x.push_back(i * res); + nm.positions.y.push_back(j * res); + nm.positions.z.push_back(0.0f); + } + } + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + const float bx = std::fmod((i + 0.5f) * res, 20.0f), by = std::fmod((j + 0.5f) * res, 20.0f); + if (bx > 3.0f && bx < 17.0f && by > 3.0f && by < 17.0f) {continue;} + const uint32_t a = j * (n + 1) + i, b = a + 1, c = a + n + 1, d = c + 1; + nm.add_navcel_to_surface(s, nm.add_navcel(a, b, d)); + nm.add_navcel_to_surface(s, nm.add_navcel(a, d, c)); + } + } + 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; +} + +// 60 x 20 m at 0.5 m: flat up to x = 20, a ramp down to x = 40 (1 in 4: 14 deg), flat again +float ramp_z(float x) {return x < 20.0f ? 0.0f : (x > 40.0f ? -5.0f : -0.25f * (x - 20.0f));} + +std::string ramp_file() +{ + NavMap nm; + const auto s = nm.create_surface("map"); + const float res = 0.5f; + const int nx = 120, ny = 40; + for (int j = 0; j <= ny; ++j) { + for (int i = 0; i <= nx; ++i) { + nm.positions.x.push_back(i * res); + nm.positions.y.push_back(j * res - 10.0f); + nm.positions.z.push_back(ramp_z(i * res)); + } + } + for (int j = 0; j < ny; ++j) { + for (int i = 0; i < nx; ++i) { + const uint32_t a = j * (nx + 1) + i, b = a + 1, c = a + nx + 1, d = c + 1; + nm.add_navcel_to_surface(s, nm.add_navcel(a, b, d)); + nm.add_navcel_to_surface(s, nm.add_navcel(a, d, c)); + } + } + const auto path = (std::filesystem::temp_directory_path() / + ("bigmap_plugin_ramp_" + 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; +} + +float longest_edge(const NavMap & nm, NavCelId c) +{ + float l = 0.0f; + for (int k = 0; k < 3; ++k) { + l = std::max( + l, (nm.positions.at(nm.navcels[c].v[k]) - + nm.positions.at(nm.navcels[c].v[(k + 1) % 3])).norm()); + } + return l; +} + +class BigMapPluginTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + easynav::RTTFBuffer::getInstance()->clear(); + map_file_ = town_file(); + set("cell_size.min", 0.5); + set("cell_size.max", 4.0); + set("min_radius", 10.0); + set("max_radius", 40.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); + } + + void TearDown() override {std::filesystem::remove(map_file_);} + + 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; + overrides_.emplace_back("bigmap.navmap_path_file", rclcpp::ParameterValue(map_file_)); + 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(double x, double y, double z = 0.0) + { + nav_msgs::msg::Odometry o; + o.header.frame_id = "map"; + o.pose.pose.position.x = x; + o.pose.pose.position.y = y; + o.pose.pose.position.z = z; + o.pose.pose.orientation.w = 1.0; // facing +x + nav_state_.set("robot_pose", o); + // and in TF (the perceptions are taken to the map frame with it) + const auto & tf_info = easynav::RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = tf_info.map_frame; + t.child_frame_id = tf_info.robot_footprint_frame; + t.transform.translation.x = x; + t.transform.translation.y = y; + t.transform.translation.z = z; + t.transform.rotation.w = 1.0; + easynav::RTTFBuffer::getInstance()->setTransform(t, "test", true); + } + + // What the robot sees in its footprint frame: a box 0.6 x 0.6 x 1 m 3 m ahead, or the ground + // (with ground_ahead, the ground from 1 to 6 m ahead, 2 m wide, at height 0 in that frame) + 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 (ground_ahead_) { + for (float x = 1.0f; x <= 6.0f; x += 0.1f) { + for (float y = -1.0f; y <= 1.0f; y += 0.1f) {p.data.push_back(pcl::PointXYZ(x, y, 0.0f));} + } + } + 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)); + } + nav_state_.set("scan", p); + } + + // update() for `seconds` of wall time (the plugin's clock) + void run(BigMapMapsManager & 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)); + } + } + + const NavMap & map() const {return nav_state_.get("map.navmap");} + + // The NavCel at (x, y) + bool at(double x, double y, NavCelId & c) const + { + std::size_t s; + Eigen::Vector3f bary; + return map().locate_navcel( + Eigen::Vector3f(static_cast(x), static_cast(y), 0.0f), s, c, bary, nullptr); + } + + std::string map_file_; + bool ground_ahead_ = false; + std::vector overrides_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + easynav::NavState nav_state_; +}; + +} // namespace + +TEST_F(BigMapPluginTest, FineAtTheBordersNearTheRobotCoarseFar) +{ + auto p = make(); + ASSERT_EQ(p->lod().levels(), 4); // 0.5, 1, 2, 4 m + robot_at(10.0, 20.0); // in a street + p->update(nav_state_); + ASSERT_TRUE(nav_state_.has("map.navmap")); + const auto & nm = map(); + EXPECT_LT(nm.navcels.size(), 20000u); // the town has 49 000 + // Next to a building near the robot: small NavCels + NavCelId c; + ASSERT_TRUE(at(10.0, 17.2, c)); + EXPECT_LE(longest_edge(nm, c), 0.5f + 1e-3f); + // In the middle of a street crossing far away: large ones + ASSERT_TRUE(at(100.0, 100.0, c)); + EXPECT_GT(longest_edge(nm, c), 2.0f); + // The filters ran on it + EXPECT_TRUE(nm.has_layer("inflated_obstacles")); + EXPECT_TRUE(nm.has_layer("obstacles")); +} + +TEST_F(BigMapPluginTest, TheMapFollowsTheRobot) +{ + auto p = make(); + robot_at(10.0, 20.0); + p->update(nav_state_); + // 0.2 m from the block of x, y in [103, 117] + NavCelId c; + ASSERT_TRUE(at(104.0, 102.8, c)); + EXPECT_GT(longest_edge(map(), c), 0.5f + 1e-3f) << "far from the robot"; + robot_at(100.0, 100.0); + run(*p, false, 0.7); // the map is made again at most every 0.5 s + ASSERT_TRUE(at(104.0, 102.8, c)); + EXPECT_LE(longest_edge(map(), c), 0.5f + 1e-3f) << "next to the robot now"; +} + +TEST_F(BigMapPluginTest, ASensedObstacleIsAddedFineAndForgottenFarAway) +{ + auto p = make(); + robot_at(10.0, 20.0); // facing +x along the street: the box at (13, 20) + run(*p, true, 0.8); + ASSERT_GT(p->sensed().size(), 0u); + NavCelId c; + ASSERT_TRUE(at(13.0, 20.0, c)); + EXPECT_EQ(map().layer_get("obstacles", c), navmap::LETHAL_OBSTACLE); + // what is marked is fine (it is near the robot) + std::size_t marked = 0; + for (NavCelId k = 0; k < map().navcels.size(); ++k) { + if (map().layer_get("obstacles", k) != navmap::LETHAL_OBSTACLE) {continue;} + ++marked; + EXPECT_LE(longest_edge(map(), k), 0.5f + 1e-3f); + } + EXPECT_GT(marked, 2u); + ASSERT_TRUE(at(13.0, 20.9, c)); // 0.6 m from the box + EXPECT_GT(map().layer_get("inflated_obstacles", c), 0) << "inflated around it"; + ASSERT_TRUE(at(13.0, 22.8, c)); + EXPECT_EQ(map().layer_get("obstacles", c), 0) << "only where it is"; + + // Farther than forget_distance (max_radius: 40 m) it is forgotten + robot_at(100.0, 100.0); + run(*p, false, 0.7); + EXPECT_EQ(p->sensed().size(), 0u); + robot_at(10.0, 20.0); + run(*p, false, 0.7); + ASSERT_TRUE(at(13.0, 20.0, c)); + EXPECT_EQ(map().layer_get("obstacles", c), 0); +} + +TEST_F(BigMapPluginTest, AnObstacleThatMovesAwayIsCleared) +{ + auto p = make(); + robot_at(10.0, 20.0); // facing +x along the street: the box at (13, 20) + run(*p, true, 0.8); + ASSERT_GT(p->sensed().size(), 0u); + NavCelId c; + ASSERT_TRUE(at(13.0, 20.0, c)); + ASSERT_EQ(map().layer_get("obstacles", c), navmap::LETHAL_OBSTACLE); + // It goes away: the robot sees the ground where it was (rays through its cells) + ground_ahead_ = true; + run(*p, false, 0.8); + EXPECT_EQ(p->sensed().size(), 0u); + ASSERT_TRUE(at(13.0, 20.0, c)); + EXPECT_NE(map().layer_get("obstacles", c), navmap::LETHAL_OBSTACLE); +} + +TEST_F(BigMapPluginTest, WithoutClearingAnObstacleIsRemembered) +{ + set("memory.clearing", false); + auto p = make(); + robot_at(10.0, 20.0); + run(*p, true, 0.8); + ASSERT_GT(p->sensed().size(), 0u); + ground_ahead_ = true; + run(*p, false, 0.8); + EXPECT_GT(p->sensed().size(), 0u) << "only the distance forgets it"; +} + +TEST_F(BigMapPluginTest, AnObstacleNotSeenAgainDecays) +{ + set("memory.decay_time", 0.3); + auto p = make(); + robot_at(10.0, 20.0); + run(*p, true, 0.5); + ASSERT_GT(p->sensed().size(), 0u); + // Out of sight (the sensor sees only the ground at its feet): forgotten after 0.3 s + run(*p, false, 0.8); + EXPECT_EQ(p->sensed().size(), 0u); +} + +TEST_F(BigMapPluginTest, GoingDownARampTheRampAheadIsNotAnObstacle) +{ + // On a ramp the robot is tilted like it, and its footprint frame (flat in TF) too: the ramp + // ahead is at height 0 in it, below the robot's own ground height + std::filesystem::remove(map_file_); + map_file_ = ramp_file(); + auto p = make(); + ground_ahead_ = true; + robot_at(25.0, 0.0, ramp_z(25.0f)); // on the ramp, facing down it (+x) + run(*p, false, 0.8); + EXPECT_EQ(p->sensed().size(), 0u); + // An obstacle on the ramp is still seen + robot_at(25.0, 0.0, ramp_z(25.0f) + 0.01); + run(*p, true, 0.8); + EXPECT_GT(p->sensed().size(), 0u); +} + +TEST_F(BigMapPluginTest, ThePublishedMapHasTheFiltersLayersAndTheLevelsAreDrawn) +{ + auto p = make(); + auto listener = rclcpp::Node::make_shared("bigmap_plugin_test_listener"); + navmap_ros_interfaces::msg::NavMap::SharedPtr got; + visualization_msgs::msg::MarkerArray::SharedPtr markers; + const std::string base = std::string(node_->get_fully_qualified_name()) + "/bigmap"; + auto sub = listener->create_subscription( + base + "/map", rclcpp::QoS(1).transient_local().reliable(), + [&](navmap_ros_interfaces::msg::NavMap::SharedPtr m) {got = m;}); + auto msub = listener->create_subscription( + base + "/markers", rclcpp::QoS(1).transient_local().reliable(), + [&](visualization_msgs::msg::MarkerArray::SharedPtr m) {markers = m;}); + robot_at(10.0, 20.0); + p->update(nav_state_); + for (int i = 0; i < 50 && (!got || !markers); ++i) { + rclcpp::spin_some(listener); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + ASSERT_TRUE(got); + bool inflated = false; + for (const auto & l : got->layers) { + inflated |= l.name == "inflated_obstacles"; + } + EXPECT_TRUE(inflated) << "the map is published after the filters"; + ASSERT_TRUE(markers); + std::size_t circles = 0; + for (const auto & m : markers->markers) { + circles += m.ns == "level radii" && m.type == visualization_msgs::msg::Marker::LINE_STRIP; + } + EXPECT_EQ(circles, 3u); // where levels 1, 2 and 3 start +} diff --git a/maps_managers/easynav_bigmap_maps_manager/test/test_tiled_lod.cpp b/maps_managers/easynav_bigmap_maps_manager/test/test_tiled_lod.cpp new file mode 100644 index 00000000..85fbbb1a --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_tiled_lod.cpp @@ -0,0 +1,260 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 "easynav_bigmap_maps_manager/tiled_lod.hpp" +#include "navmap_core/NavMap.hpp" + +using easynav::bigmap::LodParams; +using easynav::bigmap::TiledLod; +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +// Bumpy ground: the finer levels keep the bumps, the coarser ones flatten them +float ground(float x, float y) {return 0.3f * std::sin(0.6f * x) * std::cos(0.45f * y);} + +// A town of side x side m at `res` m: ground everywhere but the buildings (cells where +// building(x, y)) +NavMap town(float side, float res, const std::function & building) +{ + NavMap nm; + const auto s = nm.create_surface("map"); + const int n = static_cast(std::round(side / res)); + for (int j = 0; j <= n; ++j) { + for (int i = 0; i <= n; ++i) { + nm.positions.x.push_back(i * res); + nm.positions.y.push_back(j * res); + nm.positions.z.push_back(ground(i * res, j * res)); + } + } + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + if (building((i + 0.5f) * res, (j + 0.5f) * res)) {continue;} + const uint32_t a = j * (n + 1) + i, b = a + 1, c = a + n + 1, d = c + 1; + nm.add_navcel_to_surface(s, nm.add_navcel(a, b, d)); + nm.add_navcel_to_surface(s, nm.add_navcel(a, d, c)); + } + } + nm.rebuild_geometry_accels(); + return nm; +} + +// Blocks of 14 x 14 m every 20 m (6 m streets), and a narrow passage (1.5 m) cut through one +bool blocks(float x, float y) +{ + const float bx = std::fmod(x, 20.0f), by = std::fmod(y, 20.0f); + const bool in_block = bx > 3.0f && bx<17.0f && by>3.0f && by < 17.0f; + const bool passage = x > 49.25f && x<50.75f && y>40.0f && y < 60.0f; + return in_block && !passage; +} + +// Pieces of the map (NavCels sharing an edge) and length of the open edges +std::pair pieces_and_border(const NavMap & nm) +{ + std::map, std::vector> edges; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + for (int k = 0; k < 3; ++k) { + const uint32_t a = nm.navcels[c].v[k], b = nm.navcels[c].v[(k + 1) % 3]; + edges[{std::min(a, b), std::max(a, b)}].push_back(c); + } + } + std::vector parent(nm.navcels.size()); + for (std::size_t i = 0; i < parent.size(); ++i) { + parent[i] = static_cast(i); + } + std::function find = [&](int x) { + return parent[x] == x ? x : parent[x] = find(parent[x]); + }; + double border = 0.0; + for (const auto & [e, cels] : edges) { + if (cels.size() == 1) { + border += (nm.positions.at(e.first) - nm.positions.at(e.second)).norm(); + } + for (std::size_t i = 1; i < cels.size(); ++i) { + parent[find(cels[i])] = find(cels[0]); + } + } + int pieces = 0; + for (std::size_t i = 0; i < parent.size(); ++i) { + pieces += find(static_cast(i)) == static_cast(i); + } + return {pieces, border}; +} + +bool on_map(const NavMap & nm, float x, float y) +{ + std::size_t s; + NavCelId c; + Eigen::Vector3f bary; + return nm.locate_navcel(Eigen::Vector3f(x, y, ground(x, y) + 0.5f), s, c, bary, nullptr); +} + +LodParams params() +{ + LodParams p; + p.cell_min = 0.5; + p.cell_max = 4.0; + p.min_radius = 10.0; + p.max_radius = 40.0; + return p; +} + +} // namespace + +TEST(TiledLod, TheLevelsAndTheirRadii) +{ + TiledLod lod; + lod.build(town(20.0f, 0.5f, [](float, float) {return false;}), params()); + ASSERT_EQ(lod.levels(), 4); // 0.5, 1, 2, 4 m + EXPECT_DOUBLE_EQ(lod.cell(0), 0.5); + EXPECT_DOUBLE_EQ(lod.cell(3), 4.0); + EXPECT_DOUBLE_EQ(lod.radius(0), 0.0); + EXPECT_DOUBLE_EQ(lod.radius(1), 10.0); + EXPECT_DOUBLE_EQ(lod.radius(3), 40.0); + EXPECT_DOUBLE_EQ(lod.radius(2), 20.0); // geometric between them +} + +TEST(TiledLod, FineNearTheRobotCoarseFarAndNoCracks) +{ + const NavMap map = town(100.0f, 0.5f, blocks); + const auto [map_pieces, map_border] = pieces_and_border(map); + ASSERT_EQ(map_pieces, 1); + TiledLod lod; + const auto st = lod.build(map, params(), 4); + ASSERT_EQ(st.navcels.size(), 4u); + EXPECT_GT(st.tiles, 50u); + for (std::size_t k = 1; k < st.navcels.size(); ++k) { + EXPECT_LT(st.navcels[k], st.navcels[k - 1]) << "level " << k; + } + + const auto tiles = lod.select(1.0, 1.0); + int fine = 0, coarse = 0; + for (const auto t : tiles) { + fine += lod.level(t) == 0; + coarse += lod.level(t) == lod.levels() - 1; + } + EXPECT_GT(fine, 0); + EXPECT_GT(coarse, 0); + const NavMap nm = lod.assemble(tiles); + EXPECT_LT(nm.navcels.size(), st.navcels[0] * 6 / 10); + EXPECT_LT(nm.navcels.size(), map.navcels.size() / 10); + + // One piece, and about the same border as the map: tiles of different levels meet whole + const auto [pieces, border] = pieces_and_border(nm); + EXPECT_EQ(pieces, 1); + EXPECT_NEAR(border, map_border, 0.05 * map_border); + + // The buildings are still holes, the streets ground, the passage open, also far away + EXPECT_FALSE(on_map(nm, 10.0f, 10.0f)); + EXPECT_FALSE(on_map(nm, 90.0f, 90.0f)); + EXPECT_TRUE(on_map(nm, 1.5f, 50.0f)); + EXPECT_TRUE(on_map(nm, 99.0f, 99.0f)); + EXPECT_TRUE(on_map(nm, 50.0f, 50.0f)); +} + +TEST(TiledLod, EveryPlaceOfTheMapIsInExactlyOneTile) +{ + const NavMap map = town(100.0f, 0.5f, blocks); + TiledLod lod; + lod.build(map, params(), 4); + for (const auto & robot : {Eigen::Vector2d(1.0, 1.0), Eigen::Vector2d(50.0, 50.0), + Eigen::Vector2d(99.0, 30.0)}) + { + const NavMap nm = lod.assemble(lod.select(robot.x(), robot.y())); + double area = 0.0; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + area += nm.navcels[c].area; + } + double map_area = 0.0; + for (NavCelId c = 0; c < map.navcels.size(); ++c) { + map_area += map.navcels[c].area; + } + EXPECT_NEAR(area, map_area, 0.03 * map_area) << robot.transpose(); + EXPECT_EQ(pieces_and_border(nm).first, 1) << robot.transpose(); + } +} + +TEST(TiledLod, FloorsOverEachOtherStaySeparate) +{ + // Two floors 2.5 m apart over the same 100 x 100 m, with buildings in different places, so + // their tiles keep different vertices on the same grid lines + NavMap map = town(100.0f, 0.5f, blocks); + const NavMap upper = town(100.0f, 0.5f, [](float x, float y) {return blocks(y, x + 7.0f);}); + const auto base = static_cast(map.positions.size()); + for (std::size_t i = 0; i < upper.positions.size(); ++i) { + map.positions.x.push_back(upper.positions.x[i]); + map.positions.y.push_back(upper.positions.y[i]); + map.positions.z.push_back(upper.positions.z[i] + 2.5f); + } + const auto s = map.create_surface("upper"); + for (const auto & c : upper.navcels) { + map.add_navcel_to_surface(s, map.add_navcel(base + c.v[0], base + c.v[1], base + c.v[2])); + } + map.rebuild_geometry_accels(); + const auto [map_pieces, map_border] = pieces_and_border(map); + ASSERT_EQ(map_pieces, 2); + + TiledLod lod; + lod.build(map, params(), 4); + for (const auto & robot : {Eigen::Vector2d(1.0, 1.0), Eigen::Vector2d(50.0, 50.0)}) { + const auto tiles = lod.select(robot.x(), robot.y()); + NavMap nm = lod.assemble(tiles); + lod.detail(nm, robot.x(), robot.y(), {}, 0.6); + // No NavCel joins the floors, and each floor is whole (no cracks between its tiles) + double span = 0.0; + for (const auto & c : nm.navcels) { + const float z0 = nm.positions.z[c.v[0]], z1 = nm.positions.z[c.v[1]], + z2 = nm.positions.z[c.v[2]]; + span = std::max(span, static_cast(std::max({z0, z1, z2}) - std::min({z0, z1, z2}))); + } + EXPECT_LT(span, 1.0) << robot.transpose(); + const auto [pieces, border] = pieces_and_border(nm); + EXPECT_EQ(pieces, 2) << robot.transpose(); + EXPECT_NEAR(border, map_border, 0.05 * map_border) << robot.transpose(); + } +} + +TEST(TiledLod, TheLayersOfTheMapAreKept) +{ + NavMap map = town(40.0f, 0.5f, [](float, float) {return false;}); + auto layer = map.add_layer("obstacles"); + for (NavCelId c = 0; c < map.navcels.size(); ++c) { + const auto q = map.navcel_centroid(c); + layer->data()[c] = (q.x() > 30.0f && q.x() < 32.0f && q.y() > 5.0f && q.y() < 35.0f) ? 254 : 0; + } + TiledLod lod; + lod.build(map, params()); + const NavMap nm = lod.assemble(lod.select(0.0, 0.0)); + ASSERT_TRUE(nm.has_layer("obstacles")); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto q = nm.navcel_centroid(c); + const bool inside = q.x() > 30.1f && q.x() < 31.9f && q.y() > 5.1f && q.y() < 34.9f; + const bool outside = q.x() < 29.9f || q.x() > 32.1f || q.y() < 4.9f || q.y() > 35.1f; + if (inside) {EXPECT_EQ(nm.layer_get("obstacles", c), 254);} + if (outside) {EXPECT_EQ(nm.layer_get("obstacles", c), 0);} + } +} diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index aa4e9e26..dae312ad 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -153,12 +153,15 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) std::vector<::navmap::NavCelId> marked; // Only the points around the robot and below its height (robot frame), downsampled first - // (indices only) so that fewer points are transformed. + // (indices only) so that fewer points are transformed. Each perception with the robot's pose + // when it was taken (TF at its stamp, exact_time): with the latest pose, a cloud taken while the + // robot turns lands rotated, and walls seen a fraction of a second ago fall on free ground as + // obstacles (out of the sensor's range they are never cleared). const auto & points = PointPerceptionsOpsView(perceptions) .downsample(downsample_resolution_) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_frame, true) .filter({-max_range_, -max_range_, NAN}, {max_range_, max_range_, max_height_}, false) - .fuse(map_frame) + .fuse(map_frame, true) .as_points(); // The robot's position, for min_height_per_meter and for the clearing rays