From 6da8d37ff6ef55c4f172ca5c0145f13e365c5dba Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 17 Nov 2026 22:42:23 +0100 Subject: [PATCH 1/9] easynav_bigmap_maps_manager: a large NavMap, fine around the robot and coarse far from it The NavMap is cut into tiles and simplified into a quadtree of levels of detail once; every cycle the coarsest tile each place allows is put together around the robot (stitched, no cracks), the borders of the map and the sensed obstacles near it refined as far as the inflation reaches, and the sensed obstacles forgotten beyond forget_distance. Configuration: the map, cell_size min / max, min_radius, max_radius, forget_distance and the filters. Needs navmap_core's keep_vertices, cut_along_lines and refine (NavMap PR). --- .github/workflows/rolling.yaml | 2 +- README.md | 1 + .../CMakeLists.txt | 100 +++ .../easynav_bigmap_maps_manager/README.md | 113 +++ .../easynav_bigmap_maps_manager_plugins.xml | 9 + .../BigMapMapsManager.hpp | 111 +++ .../sensed_obstacles.hpp | 95 +++ .../easynav_bigmap_maps_manager/tiled_lod.hpp | 132 ++++ .../easynav_bigmap_maps_manager/package.xml | 35 + .../easynav_bigmap_maps_manager/pixi.toml | 7 + .../src/BigMapMapsManager.cpp | 385 ++++++++++ .../src/navmap_filter_base.cpp | 55 ++ .../src/tiled_lod.cpp | 713 ++++++++++++++++++ .../test/test_bigmap_plugin.cpp | 290 +++++++ .../test/test_tiled_lod.cpp | 219 ++++++ 15 files changed, 2266 insertions(+), 1 deletion(-) create mode 100644 maps_managers/easynav_bigmap_maps_manager/CMakeLists.txt create mode 100644 maps_managers/easynav_bigmap_maps_manager/README.md create mode 100644 maps_managers/easynav_bigmap_maps_manager/easynav_bigmap_maps_manager_plugins.xml create mode 100644 maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/BigMapMapsManager.hpp create mode 100644 maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/sensed_obstacles.hpp create mode 100644 maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/tiled_lod.hpp create mode 100644 maps_managers/easynav_bigmap_maps_manager/package.xml create mode 100644 maps_managers/easynav_bigmap_maps_manager/pixi.toml create mode 100644 maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp create mode 100644 maps_managers/easynav_bigmap_maps_manager/src/navmap_filter_base.cpp create mode 100644 maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp create mode 100644 maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp create mode 100644 maps_managers/easynav_bigmap_maps_manager/test/test_tiled_lod.cpp 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..fc1ed90e --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/README.md @@ -0,0 +1,113 @@ +# 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 200 to +1500 m, the same A*, the robot replanning every 30 m on the map made around it: + +| 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 | 620 ms | 1,166,402 | +| grid 1 m | 3,275,294 | 77 | 21/30 | 0 | 1.006 | 90 ms | 190,060 | +| grid 2 m | 744,768 | 18 | 28/30 | 0 | 1.008 | 25 ms | 71,510 | +| grid 4 m | 156,118 | 3.9 | 13/30 | 0 | 1.354 | 10 ms | 24,202 | +| bigmap (0.8 to 8 m, 20 to 300 m) | ~135,000 | 3.5 | 30/30 | 0 | 1.007 | 6.8 ms | 8,528 | + +The same paths as on the full map, 90 times faster, expanding 137 times fewer NavCels, on a map +92 times smaller, made again in ~0.3 s every 30 m. Coarser fixed grids close streets (1 m and 4 m +fail routes) or go around them (4 m: 35 % longer). + +## 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. Three more parameters have defaults that suit most robots: +`obstacle_min_height` (0.1 m: points lower above the map's surface are the ground), +`obstacle_max_height` (2.0 m: higher points do not touch the robot) and `sensor_range` (30 m, at most +`forget_distance`). + +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: 12 s for the 2.5 M NavCels of the warehouse, 40 s for the 13.8 M of +Madrid, on every core): + +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` and `obstacle_max_height` above the map's surface under them. +A point over no surface (a hole: a rack, a building) is the map's own obstacle and is left out. They +are remembered by cells of `cell_size.min / 2` until the robot is `forget_distance` from them. + +## 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 | + +## 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). 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..c2b234dc --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/BigMapMapsManager.hpp @@ -0,0 +1,111 @@ +// 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 "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}; + + // 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_; + 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..170cf05c --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/sensed_obstacles.hpp @@ -0,0 +1,95 @@ +// 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: remembered by cells + * (one point each, on the ground under the obstacle) until the robot is far from them. + */ + +#ifndef EASYNAV_BIGMAP_MAPS_MANAGER__SENSED_OBSTACLES_HPP_ +#define EASYNAV_BIGMAP_MAPS_MANAGER__SENSED_OBSTACLES_HPP_ + +#include +#include +#include +#include +#include + +#include "Eigen/Core" + +namespace easynav +{ +namespace bigmap +{ + +class SensedObstacles +{ +public: + /// Cells of `cell` m on the plane (1 m in height, so floors one over another stay apart) + explicit SensedObstacles(double cell = 0.5) + : cell_(cell) {} + + /// Remembers an obstacle at q (on the ground under it). True if its cell is new. + bool add(const Eigen::Vector3f & q) + { + return cells_.emplace(key(q), q).second; + } + + /// 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.x() - x, it->second.y() - y) > d) { + it = cells_.erase(it); + ++n; + } else { + ++it; + } + } + return n; + } + + std::vector points() const + { + std::vector out; + out.reserve(cells_.size()); + for (const auto & kv : cells_) { + out.push_back(kv.second); + } + return out; + } + + std::size_t size() const {return cells_.size();} + void clear() {cells_.clear();} + +private: + std::int64_t key(const Eigen::Vector3f & q) const + { + const auto i = static_cast(std::floor(q.x() / cell_)) & 0x1fffff; + const auto j = static_cast(std::floor(q.y() / cell_)) & 0x1fffff; + const auto k = static_cast(std::floor(q.z())) & 0x1fffff; + return (i << 42) | (j << 21) | k; + } + + double cell_; + 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..3c07fb23 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/tiled_lod.hpp @@ -0,0 +1,132 @@ +// 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). + Stats build(const ::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..1a90577b --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -0,0 +1,385 @@ +// 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 +#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_); + + // Built once, on every core: a startup step, not part of the cycle + const auto st = lod_.build(source, lp, std::max(1u, std::thread::hardware_concurrency())); + 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.size(), 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); + 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()); +} + +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 robot's footprint frame (z: height over the ground under the robot), cut to the + // box around it first, then thinned + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + const double r = sensor_range_; + PointPerceptionsOpsView view(perceptions); + view.fuse(tf_info.robot_footprint_frame); + view.filter({-r, -r, min_height_ - 1.0}, {r, r, 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 + std::size_t s; + ::navmap::NavCelId cid; + Eigen::Vector3f bary, hit; + float ground = static_cast(robot.z()); + if (map_.locate_navcel(robot.cast(), s, cid, bary, &hit)) {ground = hit.z();} + const float c = static_cast(std::cos(yaw)), sn = static_cast(std::sin(yaw)); + bool added = false; + for (const auto & pt : cloud.points) { + const Eigen::Vector3f q( + static_cast(robot.x()) + c * pt.x - sn * pt.y, + static_cast(robot.y()) + sn * pt.x + c * pt.y, ground + pt.z); + // 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(pt.x, pt.y); + if (h < min_height_ + min_height_per_m_ * range || h > max_height_) {continue;} + // 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;} + added |= sensed_.add(hit); + } + return added; +} + +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()); + 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();} + } + + 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_ + const auto tiles = lod_.select(robot.x(), robot.y()); + 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(); + for (const auto & filter : filters_) { + filter->set_map_resolution(static_cast(lod_.cell(0))); + filter->update(nav_state); + if (subscribers == 0 || !filter->is_adding_layer()) {continue;} + const auto & nm = nav_state.get<::navmap::NavMap>("map.navmap"); + const auto layer = nm.layers.get(filter->get_layer_name()); + if (!layer || (!layer->has_changes() && !new_subscriber)) {continue;} + layer_updates_pub_->publish( + navmap_ros::to_layer_update_msg( + nm, filter->get_layer_name(), header, new_subscriber ? 0.0f : 0.25f)); + } + 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..5fa30dcd --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp @@ -0,0 +1,713 @@ +// 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; +} + +// 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(const 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 = map; + ::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); + for (const auto & [ij, cels] : squares) { + Node node; + node.level = 0; + node.lines = {ij.first, ij.first + 1, ij.second, ij.second + 1}; + node.mesh = subset(cut, cels, surf); + nodes_.push_back(std::move(node)); + } + st.tiles = nodes_.size(); + cut = NavMap(); + parallel_for(nodes_.size(), threads, [this](std::size_t i) {simplify_node(nodes_[i]);}); + + // 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); +} + +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 other tiles have on it, between its ends + const float ztol = static_cast(0.5 + cell(top_) / 2.0); + auto between = [&](std::uint32_t a, std::uint32_t b, std::vector & out) { + out.clear(); + const Eigen::Vector3f pa = P[a], pb = P[b]; + const std::vector * line = nullptr; + float sa, sb; + if (pa.x() == pb.x()) { + const int ix = line_index(xs_, pa.x()); + if (ix >= 0) {line = &vlines[ix]; sa = pa.y(); sb = pb.y();} + } + if (!line && pa.y() == pb.y()) { + const int iy = line_index(ys_, pa.y()); + if (iy >= 0) {line = &hlines[iy]; sa = pa.x(); sb = pb.x();} + } + if (!line || sa == sb) {return;} + const float lo = std::min(sa, sb), hi = std::max(sa, sb); + auto it = std::upper_bound( + line->begin(), line->end(), lo, [](float s, const OnLine & o) {return s < o.s;}); + for (; it != line->end() && it->s < hi; ++it) { + const float t = (it->s - sa) / (sb - sa); + if (std::abs(P[it->v].z() - (pa.z() + t * (pb.z() - pa.z()))) > ztol) {continue;} + out.push_back(it->v); + } + if (sa > sb) {std::reverse(out.begin(), out.end());} // from a to b + }; + + 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(g[k], g[(k + 1) % 3], 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..664c14f0 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp @@ -0,0 +1,290 @@ +// 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 "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; +} + +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("navmap_path_file", map_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; + 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) + { + 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.orientation.w = 1.0; // facing +x + nav_state_.set("robot_pose", o); + } + + // What the robot sees in its footprint frame: a box 0.6 x 0.6 x 1 m 3 m ahead, or the ground + void see(bool box) + { + easynav::PointPerception p; + p.frame_id = easynav::RTTFBuffer::getInstance()->get_tf_info().robot_footprint_frame; + p.stamp = node_->now(); + p.valid = true; + if (box) { + for (float x = 2.7f; x <= 3.3f; x += 0.05f) { + for (float y = -0.3f; y <= 0.3f; y += 0.05f) { + for (float z = 0.05f; z <= 1.0f; z += 0.05f) {p.data.push_back(pcl::PointXYZ(x, y, z));} + } + } + } else { + p.data.push_back(pcl::PointXYZ(0.5f, 0.0f, 0.0f)); + } + 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_; + 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, 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..9040be13 --- /dev/null +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_tiled_lod.cpp @@ -0,0 +1,219 @@ +// 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 "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, 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);} + } +} From 44b7afeb7f43e5ccc0d9d86b4186aa41c0104e7e Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Wed, 18 Nov 2026 06:08:51 +0100 Subject: [PATCH 2/9] easynav_bigmap_maps_manager: less memory, rebuild_period parameter - Tiles kept without the NavMap accelerations (BVH, adjacency, locate grids), the map cut in place instead of copied, each tile taken from it just before it is simplified, and the memory the building threads leave free given back (malloc_trim): 40 to 70 % less resident memory. - rebuild_period (default 0.5 s), and the README of the advanced parameters. --- .../easynav_bigmap_maps_manager/README.md | 21 ++++++--- .../easynav_bigmap_maps_manager/tiled_lod.hpp | 6 ++- .../src/BigMapMapsManager.cpp | 14 +++++- .../src/tiled_lod.cpp | 46 +++++++++++++++++-- 4 files changed, 71 insertions(+), 16 deletions(-) diff --git a/maps_managers/easynav_bigmap_maps_manager/README.md b/maps_managers/easynav_bigmap_maps_manager/README.md index fc1ed90e..09b3a34d 100644 --- a/maps_managers/easynav_bigmap_maps_manager/README.md +++ b/maps_managers/easynav_bigmap_maps_manager/README.md @@ -43,10 +43,15 @@ maps_manager_node: cost_scaling_factor: 1.5 ``` -That is all it needs. Three more parameters have defaults that suit most robots: -`obstacle_min_height` (0.1 m: points lower above the map's surface are the ground), -`obstacle_max_height` (2.0 m: higher points do not touch the robot) and `sensor_range` (30 m, at most -`forget_distance`). +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 | 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). @@ -89,9 +94,11 @@ twice a second): | 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` and `obstacle_max_height` above the map's surface under them. -A point over no surface (a hole: a rack, a building) is the map's own obstacle and is left out. They -are remembered by cells of `cell_size.min / 2` until the robot is `forget_distance` from them. +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` until the robot is +`forget_distance` from them. ## Topics 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 index 3c07fb23..96c5ed4b 100644 --- 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 @@ -63,8 +63,10 @@ class TiledLod double seconds{0.0}; }; - /// Cuts @p map into tiles and simplifies every level of them (on @p threads threads). - Stats build(const ::navmap::NavMap & map, const LodParams & params, unsigned threads = 1); + /// 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;} diff --git a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp index 1a90577b..6555e5cc 100644 --- a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -20,6 +20,9 @@ #include #include #include +#if defined(__GLIBC__) +#include +#endif #include #include #include @@ -88,9 +91,16 @@ void BigMapMapsManager::on_initialize() 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_); // Built once, on every core: a startup step, not part of the cycle - const auto st = lod_.build(source, lp, std::max(1u, std::thread::hardware_concurrency())); + 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]; @@ -101,7 +111,7 @@ void BigMapMapsManager::on_initialize() } RCLCPP_INFO( node->get_logger(), "[%s] %s: %zu NavCels, %zu tiles, in %.1f s. Levels: %s", name.c_str(), - path.c_str(), source.navcels.size(), st.tiles, st.seconds, levels.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); known_ = static_cast(lod_.cell(0)); diff --git a/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp b/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp index 5fa30dcd..d00812a1 100644 --- a/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp @@ -144,6 +144,32 @@ NavMap subset( 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 { @@ -191,7 +217,7 @@ int TiledLod::level_at(double d) const return k; } -TiledLod::Stats TiledLod::build(const NavMap & map, const LodParams & params, unsigned threads) +TiledLod::Stats TiledLod::build(NavMap map, const LodParams & params, unsigned threads) { const auto t0 = std::chrono::steady_clock::now(); p_ = params; @@ -230,7 +256,7 @@ TiledLod::Stats TiledLod::build(const NavMap & map, const LodParams & params, un lines.push_back(static_cast(origin[a] + i * tile)); } } - NavMap cut = map; + 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}}); @@ -243,16 +269,25 @@ TiledLod::Stats TiledLod::build(const NavMap & map, const LodParams & params, un squares[{i, j}].push_back(c); } const auto surf = surface_of(cut); - for (const auto & [ij, cels] : squares) { + 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}; - node.mesh = subset(cut, cels, surf); 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(); - parallel_for(nodes_.size(), threads, [this](std::size_t i) {simplify_node(nodes_[i]);}); // Each level from the one below: four tiles put together, simplified with twice the error std::size_t level_begin = 0; @@ -342,6 +377,7 @@ void TiledLod::simplify_node(Node & n) const 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 From f2bb736c55e0f3e5a5683a96061e7a395b16d532 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Wed, 18 Nov 2026 09:24:11 +0100 Subject: [PATCH 3/9] easynav_bigmap_maps_manager: README with the numbers of the final city campaign --- .../easynav_bigmap_maps_manager/README.md | 30 +++++++++++-------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/maps_managers/easynav_bigmap_maps_manager/README.md b/maps_managers/easynav_bigmap_maps_manager/README.md index 09b3a34d..2c13cfb4 100644 --- a/maps_managers/easynav_bigmap_maps_manager/README.md +++ b/maps_managers/easynav_bigmap_maps_manager/README.md @@ -5,20 +5,24 @@ fine around the robot and coarse far from it, and the obstacles the sensors see 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 200 to -1500 m, the same A*, the robot replanning every 30 m on the map made around it: +(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 | 620 ms | 1,166,402 | -| grid 1 m | 3,275,294 | 77 | 21/30 | 0 | 1.006 | 90 ms | 190,060 | -| grid 2 m | 744,768 | 18 | 28/30 | 0 | 1.008 | 25 ms | 71,510 | -| grid 4 m | 156,118 | 3.9 | 13/30 | 0 | 1.354 | 10 ms | 24,202 | -| bigmap (0.8 to 8 m, 20 to 300 m) | ~135,000 | 3.5 | 30/30 | 0 | 1.007 | 6.8 ms | 8,528 | - -The same paths as on the full map, 90 times faster, expanding 137 times fewer NavCels, on a map -92 times smaller, made again in ~0.3 s every 30 m. Coarser fixed grids close streets (1 m and 4 m -fail routes) or go around them (4 m: 35 % longer). +| 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 @@ -58,8 +62,8 @@ radius also sets how far around an obstacle the NavCels are fine (see below). ## How -**Once, at start** (seconds: 12 s for the 2.5 M NavCels of the warehouse, 40 s for the 13.8 M of -Madrid, on every core): +**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 From 80731da7a880186b1410dabe07fe3c93e32f7c75 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Wed, 18 Nov 2026 10:36:47 +0100 Subject: [PATCH 4/9] easynav_bigmap_maps_manager: floors over each other are not stitched together Stitching inserted into a coarse edge on a grid line every vertex of the other side's tiles within 0.5 m plus half the coarsest cell in height (8.5 m with cells up to 16 m), so on maps with floors over each other a NavCel of one floor could be split to reach a vertex of another (in the parking garage, the roof became reachable from the floor below). Now a vertex goes, on each side of the line, into the covering edge nearest in height, and into an edge of another chain of the line only if it is within 0.5 m of it in height. Maps with one floor give the same map as before; test FloorsOverEachOtherStaySeparate. --- .../src/tiled_lod.cpp | 134 +++++++++++++++--- .../test/test_tiled_lod.cpp | 41 ++++++ 2 files changed, 155 insertions(+), 20 deletions(-) diff --git a/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp b/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp index d00812a1..34184ebf 100644 --- a/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/src/tiled_lod.cpp @@ -604,31 +604,125 @@ NavMap TiledLod::stitch(const std::vector & nodes, bool accels) c } } - // Stitching: an edge on a grid line gets the vertices other tiles have on it, between its ends - const float ztol = static_cast(0.5 + cell(top_) / 2.0); - auto between = [&](std::uint32_t a, std::uint32_t b, std::vector & out) { - out.clear(); - const Eigen::Vector3f pa = P[a], pb = P[b]; - const std::vector * line = nullptr; - float sa, sb; + // 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) {line = &vlines[ix]; sa = pa.y(); sb = pb.y();} + if (ix >= 0) { + e = {a, b, pa.y(), pb.y(), &vlines[ix], static_cast(ix), + po.x() > pa.x() ? 1 : 0}; + } } - if (!line && pa.y() == pb.y()) { + if (!e.line && pa.y() == pb.y()) { const int iy = line_index(ys_, pa.y()); - if (iy >= 0) {line = &hlines[iy]; sa = pa.x(); sb = pb.x();} + 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); } - if (!line || sa == sb) {return;} - const float lo = std::min(sa, sb), hi = std::max(sa, sb); - auto it = std::upper_bound( - line->begin(), line->end(), lo, [](float s, const OnLine & o) {return s < o.s;}); - for (; it != line->end() && it->s < hi; ++it) { - const float t = (it->s - sa) / (sb - sa); - if (std::abs(P[it->v].z() - (pa.z() + t * (pb.z() - pa.z()))) > ztol) {continue;} - out.push_back(it->v); + return it->second; + }; + std::function find = [&](std::uint32_t x) { + while (chain[x] != x) { + chain[x] = chain[chain[x]]; + x = chain[x]; } - if (sa > sb) {std::reverse(out.begin(), out.end());} // from a to b + 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; @@ -640,7 +734,7 @@ NavMap TiledLod::stitch(const std::vector & nodes, bool accels) c const auto & g = tris[t]; int edges_with = 0, which = -1; for (int k = 0; k < 3; ++k) { - between(g[k], g[(k + 1) % 3], ins[k]); + between(t, k, ins[k]); if (!ins[k].empty()) {++edges_with; which = k;} } if (edges_with == 0) { 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 index 9040be13..85fbbb1a 100644 --- a/maps_managers/easynav_bigmap_maps_manager/test/test_tiled_lod.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_tiled_lod.cpp @@ -15,6 +15,7 @@ #include +#include #include #include #include @@ -197,6 +198,46 @@ TEST(TiledLod, EveryPlaceOfTheMapIsInExactlyOneTile) } } +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;}); From ddad054ca80e81a20a41358d10badd424d09d277 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Wed, 18 Nov 2026 11:40:20 +0100 Subject: [PATCH 5/9] easynav_bigmap_maps_manager: the ramp ahead is not an obstacle going down; no layer updates before the map Sensed points were placed at the height of the ground under the robot plus their height in the footprint frame, which is flat in TF (2D odometry) while the robot stands tilted on a ramp: going down, the ramp ahead looked higher than the map there, was taken for an obstacle and blocked the ramp (in the parking garage the robot went back and forth between the two ramps down). The points are now turned by the tilt of the NavCel under the robot; test GoingDownARampTheRampAheadIsNotAnObstacle. Layer updates (map_updates) are not published while a rebuilt map waits to be published: they were of a geometry the subscribers did not have yet (RViz: "belongs to another geometry"). --- .../src/BigMapMapsManager.cpp | 34 ++++++++--- .../test/test_bigmap_plugin.cpp | 61 ++++++++++++++++++- 2 files changed, 85 insertions(+), 10 deletions(-) diff --git a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp index 6555e5cc..a5edb351 100644 --- a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -175,18 +175,34 @@ bool BigMapMapsManager::sense( get_plugin_name().c_str(), e.what()); return false; } - // The ground under the robot, on the map + // The ground under the robot, on the map, and its tilt: the robot stands on it, so on a ramp its + // footprint frame (flat in TF: the odometry is 2D) is tilted like the ramp, and the ramp ahead + // is at height 0 in it (going down a ramp, it would otherwise be taken for an obstacle above + // the ramp's lower part) std::size_t s; ::navmap::NavCelId cid; Eigen::Vector3f bary, hit; - float ground = static_cast(robot.z()); - if (map_.locate_navcel(robot.cast(), s, cid, bary, &hit)) {ground = hit.z();} - const float c = static_cast(std::cos(yaw)), sn = static_cast(std::sin(yaw)); + 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;} + tilt = Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), n); + } + } + const Eigen::Quaternionf heading( + Eigen::AngleAxisf(static_cast(yaw), Eigen::Vector3f::UnitZ())); + const Eigen::Quaternionf to_map = tilt * heading; bool added = false; for (const auto & pt : cloud.points) { - const Eigen::Vector3f q( - static_cast(robot.x()) + c * pt.x - sn * pt.y, - static_cast(robot.y()) + sn * pt.x + c * pt.y, ground + pt.z); + const Eigen::Vector3f q = ground + to_map * Eigen::Vector3f(pt.x, pt.y, pt.z); // 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;} @@ -367,7 +383,9 @@ void BigMapMapsManager::update(::easynav::NavState & nav_state) for (const auto & filter : filters_) { filter->set_map_resolution(static_cast(lod_.cell(0))); filter->update(nav_state); - if (subscribers == 0 || !filter->is_adding_layer()) {continue;} + // 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;} 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 index 664c14f0..5c5f1cd8 100644 --- a/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp @@ -79,6 +79,38 @@ std::string town_file() 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; @@ -98,7 +130,6 @@ class BigMapPluginTest : public ::testing::Test if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} easynav::RTTFBuffer::getInstance()->clear(); map_file_ = town_file(); - set("navmap_path_file", map_file_); set("cell_size.min", 0.5); set("cell_size.max", 4.0); set("min_radius", 10.0); @@ -121,6 +152,7 @@ class BigMapPluginTest : public ::testing::Test 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_)); @@ -129,23 +161,30 @@ class BigMapPluginTest : public ::testing::Test return p; } - void robot_at(double x, double y) + 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); } // 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) { @@ -181,6 +220,7 @@ class BigMapPluginTest : public ::testing::Test } std::string map_file_; + bool ground_ahead_ = false; std::vector overrides_; rclcpp_lifecycle::LifecycleNode::SharedPtr node_; easynav::NavState nav_state_; @@ -256,6 +296,23 @@ TEST_F(BigMapPluginTest, ASensedObstacleIsAddedFineAndForgottenFarAway) EXPECT_EQ(map().layer_get("obstacles", c), 0); } +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(); From 508e92baad7517fb122621f8ae93dd4964abdd02 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Wed, 18 Nov 2026 15:54:10 +0100 Subject: [PATCH 6/9] easynav_bigmap_maps_manager: sensed obstacles cleared when they move away The sensed obstacles were remembered until the robot was forget_distance from them, so one that moved (a person crossing a corridor) left a trail of obstacles that could close passages around the robot. They are now an occupancy memory in log-odds, as the NavMap maps manager's obstacle filter: a point on a cell is a hit, a ray from the robot through it a miss (memory.clearing, memory.p_hit, memory.p_miss), and memory.decay_time forgets what is not seen again. The rays cross a cell if they pass within half a metre of its height. Tests: AnObstacleThatMovesAwayIsCleared, WithoutClearingAnObstacleIsRemembered, AnObstacleNotSeenAgainDecays. --- .../easynav_bigmap_maps_manager/README.md | 13 +- .../sensed_obstacles.hpp | 171 ++++++++++++++++-- .../src/BigMapMapsManager.cpp | 32 +++- .../test/test_bigmap_plugin.cpp | 41 +++++ 4 files changed, 234 insertions(+), 23 deletions(-) diff --git a/maps_managers/easynav_bigmap_maps_manager/README.md b/maps_managers/easynav_bigmap_maps_manager/README.md index 2c13cfb4..8111f44a 100644 --- a/maps_managers/easynav_bigmap_maps_manager/README.md +++ b/maps_managers/easynav_bigmap_maps_manager/README.md @@ -56,6 +56,9 @@ That is all it needs. A few more parameters have defaults that suit most robots: | `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). @@ -101,8 +104,12 @@ twice a second): 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` until the robot is -`forget_distance` from them. +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 @@ -121,4 +128,4 @@ It is built on NavMap's tools: `simplify` (with `keep_vertices`), `cut_along_lin `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). +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/include/easynav_bigmap_maps_manager/sensed_obstacles.hpp b/maps_managers/easynav_bigmap_maps_manager/include/easynav_bigmap_maps_manager/sensed_obstacles.hpp index 170cf05c..67a052e5 100644 --- 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 @@ -15,17 +15,22 @@ /** * @file sensed_obstacles.hpp - * @brief The obstacles the sensors have seen that the map does not have: remembered by cells - * (one point each, on the ground under the obstacle) until the robot is far from them. + * @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" @@ -35,17 +40,95 @@ 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) - : cell_(cell) {} + 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); + } - /// Remembers an obstacle at q (on the ground under it). True if its cell is new. - bool add(const Eigen::Vector3f & q) + /// 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) { - return cells_.emplace(key(q), q).second; + 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. @@ -53,7 +136,7 @@ class SensedObstacles { std::size_t n = 0; for (auto it = cells_.begin(); it != cells_.end(); ) { - if (std::hypot(it->second.x() - x, it->second.y() - y) > d) { + if (std::hypot(it->second.q.x() - x, it->second.q.y() - y) > d) { it = cells_.erase(it); ++n; } else { @@ -63,30 +146,88 @@ class SensedObstacles 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_) { - out.push_back(kv.second); + if (kv.second.l > 0.0f) {out.push_back(kv.second.q);} } return out; } - std::size_t size() const {return cells_.size();} + 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 { - const auto i = static_cast(std::floor(q.x() / cell_)) & 0x1fffff; - const auto j = static_cast(std::floor(q.y() / cell_)) & 0x1fffff; - const auto k = static_cast(std::floor(q.z())) & 0x1fffff; - return (i << 42) | (j << 21) | k; + 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_; - std::unordered_map cells_; + MemoryParams p_; + float l_hit_, l_miss_, l_min_, l_max_; + std::unordered_map cells_; }; } // namespace bigmap diff --git a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp index a5edb351..ffa21245 100644 --- a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -92,6 +92,19 @@ void BigMapMapsManager::on_initialize() 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(); @@ -113,7 +126,7 @@ void BigMapMapsManager::on_initialize() 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); + sensed_ = SensedObstacles(lod_.cell(0) / 2.0, mp); known_ = static_cast(lod_.cell(0)); // Filters, as in the NavMap maps manager @@ -200,7 +213,11 @@ bool BigMapMapsManager::sense( const Eigen::Quaternionf heading( Eigen::AngleAxisf(static_cast(yaw), Eigen::Vector3f::UnitZ())); const Eigen::Quaternionf to_map = tilt * heading; - bool added = false; + // 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 q = ground + to_map * Eigen::Vector3f(pt.x, pt.y, pt.z); // An obstacle is above the map's surface: over no surface (a hole, a building) it is @@ -210,7 +227,12 @@ bool BigMapMapsManager::sense( // far ground above it const float h = q.z() - hit.z(); const double range = std::hypot(pt.x, pt.y); - if (h < min_height_ + min_height_per_m_ * range || h > max_height_) {continue;} + 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; @@ -222,9 +244,9 @@ bool BigMapMapsManager::sense( known = !map_.locate_navcel(n, s, cid, nb, &nh); } if (known) {continue;} - added |= sensed_.add(hit); + hits.push_back(hit); } - return added; + return sensed_.update(hits, ground, rays, get_node()->now().seconds()); } void BigMapMapsManager::rebuild( 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 index 5c5f1cd8..ea450de7 100644 --- a/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp @@ -296,6 +296,47 @@ TEST_F(BigMapPluginTest, ASensedObstacleIsAddedFineAndForgottenFarAway) 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 From 82e2777a89dbe6ec441b7f53cf5e38a564b798e9 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Thu, 19 Nov 2026 01:49:35 +0100 Subject: [PATCH 7/9] easynav_bigmap_maps_manager: perceptions with the pose they were taken at; the cost of a cycle The perceptions were taken to the footprint frame and placed on the map with the robot's pose of now: a cloud taken while the robot turned in place landed rotated, walls seen a fraction of a second earlier fell on free ground as obstacles, and those behind racks, out of sight once the robot stopped, were never cleared (in the large warehouse the robot stayed boxed in after its first goal). They are now taken to the map frame with TF at their stamps; the tilt correction on ramps turns each point about the robot as before. For measurements: every cycle the NavState gets "map.bigmap_stats" (rebuilds so far, the last rebuild's select, assemble and detail times and NavCels, this cycle's sensing and filter times), also published on //stats while someone listens. --- .../easynav_bigmap_maps_manager/README.md | 1 + .../BigMapMapsManager.hpp | 8 +++ .../src/BigMapMapsManager.cpp | 56 ++++++++++++++----- .../test/test_bigmap_plugin.cpp | 11 ++++ 4 files changed, 63 insertions(+), 13 deletions(-) diff --git a/maps_managers/easynav_bigmap_maps_manager/README.md b/maps_managers/easynav_bigmap_maps_manager/README.md index 8111f44a..b4f6ac78 100644 --- a/maps_managers/easynav_bigmap_maps_manager/README.md +++ b/maps_managers/easynav_bigmap_maps_manager/README.md @@ -117,6 +117,7 @@ occupied than not; it is forgotten when the robot is `forget_distance` from it ( |---|---|---| | `//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 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 index c2b234dc..7aa5b6de 100644 --- 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 @@ -42,6 +42,7 @@ #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 @@ -85,6 +86,11 @@ class BigMapMapsManager : public easynav::MapsManagerBase 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 @@ -102,6 +108,8 @@ class BigMapMapsManager : public easynav::MapsManagerBase 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}; }; diff --git a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp index ffa21245..4ed455bb 100644 --- a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #if defined(__GLIBC__) #include @@ -153,6 +154,8 @@ void BigMapMapsManager::on_initialize() 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( @@ -173,13 +176,17 @@ bool BigMapMapsManager::sense( if (perceptions.empty() || !has_map_) {return false;} pcl::PointCloud cloud; try { - // In the robot's footprint frame (z: height over the ground under the robot), cut to the - // box around it first, then thinned + // In the map frame, each perception with the robot's pose when it was taken (TF at its + // stamp): with the pose of now, a cloud taken while the robot turns lands rotated, 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.robot_footprint_frame); - view.filter({-r, -r, min_height_ - 1.0}, {r, r, max_height_ + 1.0}, false); + view.fuse(tf_info.map_frame); + 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) { @@ -188,10 +195,10 @@ bool BigMapMapsManager::sense( 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 its - // footprint frame (flat in TF: the odometry is 2D) is tilted like the ramp, and the ramp ahead - // is at height 0 in it (going down a ramp, it would otherwise be taken for an obstacle above - // the ramp's lower part) + // 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 while TF is flat (the odometry is 2D), and 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 the tilt std::size_t s; ::navmap::NavCelId cid; Eigen::Vector3f bary, hit; @@ -210,23 +217,23 @@ bool BigMapMapsManager::sense( tilt = Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), n); } } - const Eigen::Quaternionf heading( - Eigen::AngleAxisf(static_cast(yaw), Eigen::Vector3f::UnitZ())); - const Eigen::Quaternionf to_map = tilt * heading; + (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 q = ground + to_map * Eigen::Vector3f(pt.x, pt.y, pt.z); + 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(pt.x, pt.y); + 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 @@ -264,6 +271,11 @@ void BigMapMapsManager::rebuild( 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(); @@ -375,6 +387,8 @@ void BigMapMapsManager::update(::easynav::NavState & nav_state) 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; @@ -382,7 +396,10 @@ void BigMapMapsManager::update(::easynav::NavState & nav_state) // 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_)) @@ -402,6 +419,7 @@ void BigMapMapsManager::update(::easynav::NavState & nav_state) 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); @@ -415,6 +433,18 @@ void BigMapMapsManager::update(::easynav::NavState & nav_state) 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) 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 index ea450de7..841b60e6 100644 --- a/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/test/test_bigmap_plugin.cpp @@ -33,6 +33,7 @@ #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" @@ -170,6 +171,16 @@ class BigMapPluginTest : public ::testing::Test 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 From beec1487abd846bba7d84cecbbf2f8cce13903b3 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Thu, 19 Nov 2026 02:23:48 +0100 Subject: [PATCH 8/9] easynav_bigmap_maps_manager: the tilt correction only for what TF lacks With the perceptions taken to the map frame by TF, a localizer that gives the robot's pose in 3D (its pitch on a ramp) already tilts them: the correction for a flat TF was applied twice and the ramps were taken for obstacles again (the parking garage with a 3D ground truth). The points are now turned from the robot's up in TF to the normal of the ground under it. --- .../src/BigMapMapsManager.cpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp index 4ed455bb..add89e05 100644 --- a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -196,9 +196,10 @@ bool BigMapMapsManager::sense( 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 while TF is flat (the odometry is 2D), and 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 the tilt + // 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; @@ -214,7 +215,18 @@ bool BigMapMapsManager::sense( if (n.norm() > 1e-9f) { n.normalize(); if (n.z() < 0.0f) {n = -n;} - tilt = Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), 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 From 239df65e7dda9906515c56451bdb772f733a65a0 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Thu, 19 Nov 2026 11:18:31 +0100 Subject: [PATCH 9/9] Sensed obstacles with the pose each cloud was taken at (exact_time) BigMap and the NavMap obstacle filter placed every cloud on the map with the robot's latest pose. A cloud may be up to the sensors' forget_time (0.5 s) old and is used in every cycle until the next one arrives, so while the robot turned in place its points landed rotated: walls 14 m away fell across free aisles as lethal obstacles, and once the robot left, out of the sensor's range, they were never cleared (in the large warehouse a robot on the fixed NavMap stayed boxed in). Both now fuse each perception with the TF at its stamp. Turning in place at 0.8 rad/s for 40 s in the warehouse, where the map has every obstacle: BigMap remembered 594 made-up obstacles before and 0 after; the NavMap filter peaked at 27 cells before and 2 after. --- .../src/BigMapMapsManager.cpp | 10 ++++++---- .../filters/ObstacleFilter.cpp | 9 ++++++--- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp index add89e05..46aecb37 100644 --- a/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp +++ b/maps_managers/easynav_bigmap_maps_manager/src/BigMapMapsManager.cpp @@ -177,13 +177,15 @@ bool BigMapMapsManager::sense( pcl::PointCloud cloud; try { // In the map frame, each perception with the robot's pose when it was taken (TF at its - // stamp): with the pose of now, a cloud taken while the robot turns lands rotated, 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. + // 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); + 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); 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