From e44a056a7cc05652873003c3e0d71848c3915da4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 12:57:43 +0200 Subject: [PATCH] NavMap maps manager: local NavMap around the robot MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Fixes #119 - Local mode (local.* parameters): the maps manager keeps a NavMap around the robot, in its frame, rebuilt at local.freq from the point perceptions in NavState (accumulated in odom over local.accumulate_time, cropped to the window and meshed with navmap_ros::from_points). Each rebuild is published and replaces the map in NavState. - A new map (received or rebuilt) now replaces the one in NavState; before, only the first one ever got there. - ObstacleFilter works in the NavMap's own frame, not always in map. - InflationFilter skips an empty NavMap instead of logging an error. Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_maps_manager/README.md | 15 ++ .../NavMapMapsManager.hpp | 31 ++++ .../NavMapMapsManager.cpp | 158 +++++++++++++++- .../filters/InflationFilter.cpp | 1 + .../filters/ObstacleFilter.cpp | 7 +- .../tests/CMakeLists.txt | 3 + .../tests/navmap_local_mode_tests.cpp | 172 ++++++++++++++++++ 7 files changed, 384 insertions(+), 3 deletions(-) create mode 100644 maps_managers/easynav_navmap_maps_manager/tests/navmap_local_mode_tests.cpp diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 5b060640..d17a327b 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -44,6 +44,21 @@ All parameters are declared under the plugin namespace, i.e. | `.pcd_path_file` | `string` | `""` | Relative path (inside the package) to a point cloud (PCD/PLY) used to build a NavMap surface. | | `.filters` | `list` | `[]` | Ordered list of filter plugin names to be applied after map loading (e.g. `["obstacles", "inflation"]`). | +### Local mode + +With `.local.enabled`, the maps manager keeps a NavMap around the robot, in its frame, rebuilt continuously from the point perceptions in NavState (scans accumulated in `odom` over `accumulate_time`, cropped to the window and meshed with `navmap_ros::from_points`). Each rebuild is published on `/map` and replaces the map in NavState; the filters run on it as usual (the ObstacleFilter works in the NavMap's own frame). A map loaded from a file is replaced at the first rebuild. + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.local.enabled` | `bool` | `false` | Enable the local mode. | +| `.local.frame` | `string` | robot frame | Frame of the NavMap (e.g. `base_footprint`, ground at z = 0). | +| `.local.range` | `double` | `10.0` | Half side (m) of the square window around the robot. | +| `.local.min_z`, `.local.max_z` | `double` | `-3.0`, `3.0` | Height band (m, in `frame`) of the points used. | +| `.local.accumulate_time` | `double` | `1.0` | Scans put together before meshing (s): a single 3D scan is sparse rings. | +| `.local.freq` | `double` | `2.0` | Rebuilds per second (`<= 0`: every update). | +| `.local.downsample` | `double` | `0.1` | Voxel (m) the accumulated points are reduced to. | +| `.local.resolution`, `max_dz`, `max_slope_deg`, `min_clearance`, `min_surface_area`, `max_hole_area` | `double` | `0.3`, `0.25`, `30`, `1.0`, `1.0`, `1.0` | Mesher parameters (`navmap_ros::BuildParams`). | + ### Filter Plugins #### **ObstacleFilter** diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp index da7afbc2..e9e3e9c3 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp @@ -19,6 +19,7 @@ #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAPMAPSMANAGER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAPMAPSMANAGER_HPP_ +#include #include #include #include @@ -28,6 +29,9 @@ #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "pcl/point_types.h" +#include "pcl/point_cloud.h" +#include "navmap_ros/conversions.hpp" #include "easynav_core/MapsManagerBase.hpp" #include "navmap_core/NavMap.hpp" #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" @@ -132,6 +136,33 @@ class NavMapMapsManager : public easynav::MapsManagerBase std::vector> navmap_filters_; double resolution_ {1.0}; + + /// Map replaced since it was last put in NavState (loaded, received or regenerated). + bool map_changed_ {true}; + + // ---- Local mode: a NavMap around the robot, in its frame, rebuilt from the sensors ---- + struct LocalParams + { + bool enabled {false}; + std::string frame; ///< NavMap frame (default: the robot frame) + double range {10.0}; ///< Half side of the square window (m) + double min_z {-3.0}; ///< Points kept in [min_z, max_z] (m, in frame) + double max_z {3.0}; + double accumulate_time {1.0}; ///< Scans put together before meshing (s) + double freq {2.0}; ///< Rebuilds per second + double downsample {0.1}; ///< Voxel of the accumulated points (m) + navmap_ros::BuildParams build; ///< Mesher parameters + } local_; + struct TimedCloud + { + double stamp; ///< s (time sources of stamps and clock may differ: compared as seconds) + pcl::PointCloud cloud; ///< In the odom frame + }; + std::deque local_scans_; + double last_local_build_ {-1e18}; ///< s + + /// Accumulates the perceptions and, at local.freq, rebuilds navmap_ around the robot. + void update_local(::easynav::NavState & nav_state); }; } // namespace navmap diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index 859db415..e065fdc1 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp @@ -13,11 +13,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include +#include #include "easynav_common/Parameters.hpp" #include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" +#include "Eigen/Geometry" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" #include "easynav_common/RTTFBuffer.hpp" @@ -75,6 +79,54 @@ NavMapMapsManager::on_initialize() node->get_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->get_parameter(plugin_name + ".navmap_path_file", navmap_path_file); + // Local mode + { + const std::string L = plugin_name + ".local."; + auto & b = local_.build; + easynav::declare_parameter_if_absent(*node, L + "enabled", local_.enabled); + easynav::declare_parameter_if_absent(*node, L + "frame", local_.frame); + easynav::declare_parameter_if_absent(*node, L + "range", local_.range); + easynav::declare_parameter_if_absent(*node, L + "min_z", local_.min_z); + easynav::declare_parameter_if_absent(*node, L + "max_z", local_.max_z); + easynav::declare_parameter_if_absent(*node, L + "accumulate_time", local_.accumulate_time); + easynav::declare_parameter_if_absent(*node, L + "freq", local_.freq); + easynav::declare_parameter_if_absent(*node, L + "downsample", local_.downsample); + double res = 0.3, max_dz = b.max_dz, slope = b.max_slope_deg, clearance = b.min_clearance; + double min_area = 1.0, hole = b.max_hole_area; + easynav::declare_parameter_if_absent(*node, L + "resolution", res); + easynav::declare_parameter_if_absent(*node, L + "max_dz", max_dz); + easynav::declare_parameter_if_absent(*node, L + "max_slope_deg", slope); + easynav::declare_parameter_if_absent(*node, L + "min_clearance", clearance); + easynav::declare_parameter_if_absent(*node, L + "min_surface_area", min_area); + easynav::declare_parameter_if_absent(*node, L + "max_hole_area", hole); + node->get_parameter(L + "enabled", local_.enabled); + node->get_parameter(L + "frame", local_.frame); + node->get_parameter(L + "range", local_.range); + node->get_parameter(L + "min_z", local_.min_z); + node->get_parameter(L + "max_z", local_.max_z); + node->get_parameter(L + "accumulate_time", local_.accumulate_time); + node->get_parameter(L + "freq", local_.freq); + node->get_parameter(L + "downsample", local_.downsample); + node->get_parameter(L + "resolution", res); + node->get_parameter(L + "max_dz", max_dz); + node->get_parameter(L + "max_slope_deg", slope); + node->get_parameter(L + "min_clearance", clearance); + node->get_parameter(L + "min_surface_area", min_area); + node->get_parameter(L + "max_hole_area", hole); + b.resolution = static_cast(res); + b.max_dz = static_cast(max_dz); + b.max_slope_deg = static_cast(slope); + b.min_clearance = static_cast(clearance); + b.min_surface_area = static_cast(min_area); + b.max_hole_area = static_cast(hole); + if (local_.frame.empty()) {local_.frame = RTTFBuffer::getInstance()->get_tf_info().robot_frame;} + if (local_.enabled) { + RCLCPP_INFO( + node->get_logger(), "[%s] local mode: NavMap in %s, %.1f m around the robot, %.1f Hz", + plugin_name.c_str(), local_.frame.c_str(), local_.range, local_.freq); + } + } + std::vector navmap_filters; easynav::declare_parameter_if_absent(*node, plugin_name + ".filters", navmap_filters); node->get_parameter(plugin_name + ".filters", navmap_filters); @@ -178,6 +230,7 @@ NavMapMapsManager::on_initialize() [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { resolution_ = msg->info.resolution; navmap_ = navmap_ros::from_occupancy_grid(*msg); + map_changed_ = true; navmap_msg_ = navmap_ros::to_msg(navmap_); navmap_msg_.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().map_frame; @@ -192,6 +245,7 @@ NavMapMapsManager::on_initialize() [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { navmap_ros::BuildParams params; navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params); + map_changed_ = true; navmap_msg_.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().map_frame; @@ -225,8 +279,12 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) { EASYNAV_TRACE_EVENT; - if (!nav_state.has("map.navmap")) { + if (local_.enabled) {update_local(nav_state);} + + // A new map (loaded, received or rebuilt) replaces the one in NavState + if (map_changed_ || !nav_state.has("map.navmap")) { nav_state.set("map.navmap", navmap_); + map_changed_ = false; } for (const auto & filter : navmap_filters_) { @@ -243,6 +301,104 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) } +void +NavMapMapsManager::update_local(::easynav::NavState & nav_state) +{ + auto node = get_node(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + const double now = node->now().seconds(); + + // 1) Accumulate the newest perceptions, in odom (it does not jump), around the robot + { + const auto & perceptions = nav_state.get_no_group(); + const double stamp = perceptions.empty() ? 0.0 : + get_latest_point_perceptions_stamp(perceptions).seconds(); + if (!perceptions.empty() && (local_scans_.empty() || stamp > local_scans_.back().stamp)) { + try { + auto cloud = PointPerceptionsOpsView(perceptions) + .downsample(local_.downsample) + .fuse(local_.frame) + .filter( + {-local_.range, -local_.range, local_.min_z}, + {local_.range, local_.range, local_.max_z}, false) + .fuse(tf_info.odom_frame) + .as_points(); + local_scans_.push_back({stamp, std::move(cloud)}); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + node->get_logger(), *node->get_clock(), 5000, "[%s] local mode: %s", + get_plugin_name().c_str(), e.what()); + } + } + } + while (!local_scans_.empty() && + now - local_scans_.front().stamp > local_.accumulate_time) + { + local_scans_.pop_front(); + } + + // 2) At local.freq: put them together in the local frame, crop, voxelize and mesh + if (local_scans_.empty() || + (local_.freq > 0.0 && now - last_local_build_ < 1.0 / local_.freq)) + { + return; + } + last_local_build_ = now; + + geometry_msgs::msg::TransformStamped tf; + try { + tf = RTTFBuffer::getInstance()->lookupTransform( + local_.frame, tf_info.odom_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); + } catch (const tf2::TransformException & e) { + RCLCPP_WARN_THROTTLE( + node->get_logger(), *node->get_clock(), 5000, "[%s] local mode: no %s -> %s: %s", + get_plugin_name().c_str(), tf_info.odom_frame.c_str(), local_.frame.c_str(), e.what()); + return; + } + const auto & q = tf.transform.rotation; + const auto & t = tf.transform.translation; + const Eigen::Quaternionf rot( + static_cast(q.w), static_cast(q.x), static_cast(q.y), + static_cast(q.z)); + const Eigen::Vector3f tr( + static_cast(t.x), static_cast(t.y), static_cast(t.z)); + const float range = static_cast(local_.range); + const float vox = static_cast(std::max(local_.downsample, 1e-3)); + + pcl::PointCloud local; + std::unordered_set voxels; + for (const auto & scan : local_scans_) { + for (const auto & p : scan.cloud.points) { + if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) {continue;} + const Eigen::Vector3f v = rot * Eigen::Vector3f(p.x, p.y, p.z) + tr; + if (std::abs(v.x()) > range || std::abs(v.y()) > range || v.z() < local_.min_z || + v.z() > local_.max_z) + { + continue; + } + const std::int64_t key = + (static_cast(std::floor(v.x() / vox)) * 73856093) ^ + (static_cast(std::floor(v.y() / vox)) * 19349663) ^ + (static_cast(std::floor(v.z() / vox)) * 83492791); + if (voxels.insert(key).second) {local.push_back(pcl::PointXYZ(v.x(), v.y(), v.z()));} + } + } + + navmap_ros_interfaces::msg::NavMap msg; + ::navmap::NavMap nm = navmap_ros::from_points(local, msg, local_.build); + if (nm.navcels.empty()) {return;} + for (auto & s : nm.surfaces) { + s.frame_id = local_.frame; + } + navmap_ = std::move(nm); + map_changed_ = true; + + navmap_msg_ = navmap_ros::to_msg(navmap_); + navmap_msg_.header.frame_id = local_.frame; + navmap_msg_.header.stamp = node->now(); + navmap_pub_->publish(navmap_msg_); +} + } // namespace navmap } // namespace easynav #include diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index 08a6687c..c94c92a1 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp @@ -167,6 +167,7 @@ void InflationFilter::update(::easynav::NavState & nav_state) } navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + if (navmap_.navcels.empty()) {return;} // no map yet (e.g. a local map before its first build) const bool ok = inflate_layer_u8( navmap_, 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 94ddf692..d3621319 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 @@ -76,6 +76,9 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + // The NavMap's own frame (a local NavMap is in the robot frame), the map frame by default + const std::string map_frame = !navmap_.surfaces.empty() && !navmap_.surfaces[0].frame_id.empty() ? + navmap_.surfaces[0].frame_id : tf_info.map_frame; // Start from the static map, if the NavMap has one (built from an occupancy grid): what the // sensors do not see right now (behind something, far, too low) is still an obstacle. @@ -96,7 +99,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) .downsample(downsample_resolution_) .fuse(tf_info.robot_frame) .filter({-max_range_, -max_range_, NAN}, {max_range_, max_range_, max_height_}, false) - .fuse(tf_info.map_frame) + .fuse(map_frame) .as_points(); // The robot's position, for min_height_per_meter @@ -105,7 +108,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) if (min_height_per_meter_ > 0.0) { try { const auto tf = RTTFBuffer::getInstance()->lookupTransform( - tf_info.map_frame, tf_info.robot_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); + map_frame, tf_info.robot_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); robot_xy = {static_cast(tf.transform.translation.x), static_cast(tf.transform.translation.y)}; robot_known = true; diff --git a/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt index 826caa97..ffb49a32 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt @@ -12,3 +12,6 @@ target_link_libraries(navmap_obstacle_filter_tests ${PROJECT_NAME}) ament_add_gtest(navmap_inflation_filter_tests navmap_inflation_filter_tests.cpp) target_link_libraries(navmap_inflation_filter_tests ${PROJECT_NAME}) + +ament_add_gtest(navmap_local_mode_tests navmap_local_mode_tests.cpp) +target_link_libraries(navmap_local_mode_tests ${PROJECT_NAME}) diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_local_mode_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_local_mode_tests.cpp new file mode 100644 index 00000000..36f3d80e --- /dev/null +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_local_mode_tests.cpp @@ -0,0 +1,172 @@ +// 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 "gtest/gtest.h" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "navmap_core/NavMap.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace +{ + +class NavMapLocalModeTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + static int count = 0; + node_ = rclcpp_lifecycle::LifecycleNode::make_shared( + "navmap_local_mode_test_" + std::to_string(count++)); + easynav::RTTFBuffer::getInstance()->clear(); + } + + void robot_at(double x) + { + auto buf = easynav::RTTFBuffer::getInstance(); + geometry_msgs::msg::TransformStamped tf; + tf.header.frame_id = buf->get_tf_info().odom_frame; + tf.child_frame_id = buf->get_tf_info().robot_frame; + tf.transform.translation.x = x; + tf.transform.rotation.w = 1.0; + buf->setTransform(tf, "test", true); + } + + // What the robot sees, in its own frame: flat ground 10 x 10 m around it, a 0.6 m box 2 m + // ahead, and a far wall 20 m away (out of the window). Stamped dt s from now. + void see(double dt) + { + easynav::PointPerception p; + p.frame_id = easynav::RTTFBuffer::getInstance()->get_tf_info().robot_frame; + p.stamp = node_->now() + rclcpp::Duration::from_seconds(dt); + p.valid = true; + for (float x = -5.0f; x <= 5.0f; x += 0.05f) { + for (float y = -5.0f; y <= 5.0f; y += 0.05f) { + const bool box = x > 1.7f && x<2.3f && y>-0.3f && y < 0.3f; + p.data.push_back(pcl::PointXYZ(x, y, box ? 0.6f : 0.0f)); + } + } + for (float z = 0.0f; z < 2.0f; z += 0.1f) {p.data.push_back(pcl::PointXYZ(20.0f, 0.0f, z));} + nav_state_.set("scan", p); + } + + std::shared_ptr make(bool enabled) + { + node_->declare_parameter("navmap.local.enabled", enabled); + node_->declare_parameter("navmap.local.range", 4.0); + node_->declare_parameter("navmap.local.freq", 0.0); // rebuild on every update + node_->declare_parameter("navmap.local.accumulate_time", 1e6); // keep every scan + auto m = std::make_shared(); + m->initialize(node_, "navmap"); + return m; + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + easynav::NavState nav_state_; +}; + +float area(const ::navmap::NavMap & nm) +{ + float a = 0.0f; + for (const auto & c : nm.navcels) { + a += c.area; + } + return a; +} + +} // namespace + +TEST_F(NavMapLocalModeTest, BuildsANavMapAroundTheRobotInItsFrame) +{ + robot_at(0.0); + auto m = make(true); + see(0.0); + m->update(nav_state_); + ASSERT_TRUE(nav_state_.has("map.navmap")); + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + ASSERT_FALSE(nm.navcels.empty()); + EXPECT_EQ(nm.surfaces[0].frame_id, easynav::RTTFBuffer::getInstance()->get_tf_info().robot_frame); + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto p = nm.navcel_centroid(c); + EXPECT_LE(std::abs(p.x()), 4.0f + 0.3f); // the window, not the far wall + EXPECT_LE(std::abs(p.y()), 4.0f + 0.3f); + } + EXPECT_GT(area(nm), 0.8f * 8.0f * 8.0f); + // The ground under the robot is there, at its height; the box is not ground + std::size_t s; + ::navmap::NavCelId cid; + Eigen::Vector3f bary, hit; + ASSERT_TRUE(nm.locate_navcel({0.0f, 0.0f, 0.1f}, s, cid, bary, &hit)); + EXPECT_NEAR(hit.z(), 0.0f, 0.05f); + const bool ground_at_box = nm.locate_navcel({2.0f, 0.0f, 0.1f}, s, cid, bary, &hit) && + std::abs(hit.z()) < 0.1f; + if (ground_at_box) { + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto q = nm.navcel_centroid(c); + if (std::abs(q.x() - 2.0f) < 0.5f && std::abs(q.y()) < 0.3f) { + ADD_FAILURE() << "cell " << c << " at " << q.transpose() << " hit " << hit.transpose(); + } + } + } + EXPECT_FALSE(ground_at_box); +} + +TEST_F(NavMapLocalModeTest, FollowsTheRobotAndReplacesTheMapInNavState) +{ + robot_at(0.0); + auto m = make(true); + see(0.0); + m->update(nav_state_); + const auto first = nav_state_.get<::navmap::NavMap>("map.navmap").positions.x; + ASSERT_FALSE(first.empty()); + // The robot moves 1 m forward and sees the same thing again (relative to itself) + robot_at(1.0); + see(0.5); + m->update(nav_state_); + const auto & nm = nav_state_.get<::navmap::NavMap>("map.navmap"); + EXPECT_NE(nm.positions.x, first); // a new map replaced the old one in NavState + for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_LE(std::abs(nm.navcel_centroid(c).x()), 4.0f + 0.3f); // still around the robot + } + // No new perception: rebuilt from the same scans, the same map + const auto second = nm.positions.x; + m->update(nav_state_); + EXPECT_EQ(nav_state_.get<::navmap::NavMap>("map.navmap").positions.x, second); +} + +TEST_F(NavMapLocalModeTest, DisabledByDefault) +{ + auto off = make(false); + robot_at(0.0); + see(0.0); + off->update(nav_state_); + EXPECT_TRUE(nav_state_.get<::navmap::NavMap>("map.navmap").navcels.empty()); +} + +TEST_F(NavMapLocalModeTest, SafeWithoutOdometry) +{ + auto on = make(true); // no odom -> robot transform at all + see(0.0); + EXPECT_NO_THROW(on->update(nav_state_)); + EXPECT_TRUE(nav_state_.get<::navmap::NavMap>("map.navmap").navcels.empty()); +}