diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 9a3550e0..52b73299 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -68,7 +68,8 @@ With `.local.enabled`, the maps manager keeps a NavMap around the robot, - **Description:** Detects occupied NavCels from the point perceptions in `NavState` and marks them as `LETHAL_OBSTACLE` in the `"obstacles"` layer, on top of the static map's `"occupancy"` layer (if any). The points are downsampled, those within `max_range` and below `max_height` (robot frame) are grouped in 0.3 m columns, and a column is an obstacle when its highest point is more than `min_height` (plus `min_height_per_meter` for each meter from the robot) above the NavMap surface under it (along the surface's normal). So a 2D laser detects obstacles too, and the ground or a ramp, being on the surface, is not one. - The layer is rebuilt from the static map only when the map changes; each cycle writes just the cells that become obstacles or stop being one. + The layer is rebuilt from the static map only when the map changes; each cycle writes just the cells that become obstacles or stop being one. + With `memory.enabled`, the evidence is accumulated per NavCel in log-odds (`"obstacles_logodds"`, int16, clamped as OctoMap): a point above the surface is a hit, the cells a ray from the robot crosses (and where a ground point lies) are misses, and unobserved cells can decay. `"obstacles_sensor"` says what the sensors know (obstacle, free or unknown), and `"obstacles"` is the static map with the sensors on top (`navmap::combine_layer`, max): the sensors fill in cells the static map does not know, and never erase a static obstacle. `NO_INFORMATION` is the declared unknown value of these layers. | Parameter | Type | Default | Description | |---|---|---:|---| @@ -77,6 +78,10 @@ With `.local.enabled`, the maps manager keeps a NavMap around the robot, | `.min_height_per_meter` | `double` | `0.0` | `min_height` grows this much per meter of horizontal distance from the robot (`< 0` or not finite: `0`). On rough terrain, a small error in the robot's tilt lifts the ground more the farther it is; e.g. `0.05` adds 0.25 m at 5 m. | | `.max_height` | `double` | `NaN` | Points above this height (robot frame) are ignored; `NaN` disables it. | | `.downsample_resolution` | `double` | `0.3` | Voxel size (m) the points are reduced to first; `<= 0` disables it. | +| `.memory.enabled` | `bool` | `false` | Remember obstacles that are out of sight (see below). Off: only what is seen in the current cycle, on top of the static map. | +| `.memory.clearing` | `bool` | `true` | Memory: rays from the robot to each point, along the surface, are evidence of free space in the cells they cross. | +| `.memory.decay_time` | `double` | `0.0` | Memory: seconds for a cell no longer observed to fade back to unknown (from the most certain occupied); `0` never. | +| `.memory.p_hit`, `memory.p_miss` | `double` | `0.7`, `0.4` | Memory: occupancy probability of a hit and of a cell a ray crosses (as OctoMap). | | **Input Key:** | | | Reads list of point clouds from `NavState` group `"points"`. | | **Output Layer:** | | | Updates or creates NavMap layer `"obstacles"`. | diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp index 5797a790..bcf02a67 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp @@ -71,8 +71,7 @@ class InflationFilter : public NavMapFilter std::unique_ptr<::navmap::DistanceField> df_; const ::navmap::NavMap * df_map_{nullptr}; std::uint64_t df_geometry_{0}; - const ::navmap::LayerViewBase * last_src_{nullptr}; - const ::navmap::LayerViewBase * last_dst_{nullptr}; + std::weak_ptr<::navmap::LayerViewBase> last_src_, last_dst_; // replaced or gone: rebuild std::vector lethal_; // obstacle cells the distance field is seeded with }; diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp index e836e888..67306d98 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp @@ -19,7 +19,9 @@ #include #include +#include #include +#include #include #include "navmap_core/NavMap.hpp" @@ -55,8 +57,25 @@ class ObstacleFilter : public NavMapFilter std::string get_layer_name() override {return "obstacles";} private: + // Memory mode: evidence accumulated in log-odds, cleared by rays, faded by decay + void update_memory( + ::navmap::NavMap & nm, const Eigen::Vector3f & robot, bool robot_known, + const std::vector<::navmap::NavCelId> & hits, + const std::vector> & ends); + + bool memory_ {false}; ///< Remember obstacles (log-odds) instead of this cycle only + bool clearing_ {true}; ///< Memory: rays from the robot clear the cells they cross + double decay_time_ {0.0}; ///< Memory: s for a certain cell to fade to unknown; 0: never + double p_hit_ {0.7}; ///< Memory: occupancy probability of a hit + double p_miss_ {0.4}; ///< Memory: occupancy probability of a cell a ray crosses + std::vector<::navmap::NavCelId> active_; // cells with evidence (non-zero log-odds) + std::vector in_active_, flag_; + std::vector stamp_; + std::uint32_t cycle_ {0}; + double last_time_ {-1.0}, decay_acc_ {0.0}; + // Incremental state, bound to the layer it was built on - const ::navmap::LayerViewBase * last_layer_{nullptr}; + std::weak_ptr<::navmap::LayerViewBase> last_layer_; // replaced or gone: rebuild std::uint64_t last_occ_hash_{0}; std::vector base_; // static value of each cell std::vector<::navmap::NavCelId> marked_; // obstacle cells of the last cycle 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 eda22032..05650803 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 @@ -195,11 +195,9 @@ void InflationFilter::update(::easynav::NavState & nav_state) const float k = static_cast(cost_scaling_factor_); auto dst = std::dynamic_pointer_cast(nm.layers.get(get_layer_name())); - // A new map, a new obstacle layer, or our layer gone or replaced (a new layer is all changed): - // start over + // A new map, a new obstacle layer, or our layer gone or replaced: start over const bool rebuild = !df_ || df_map_ != &nm || df_geometry_ != nm.geometry_id() || - last_src_ != src.get() || !dst || dst.get() != last_dst_ || dst->size() != N || - dst->all_changed(); + src != last_src_.lock() || !dst || dst != last_dst_.lock() || dst->size() != N; if (rebuild) { if (!dst || dst->size() != N) { nm.layers.remove(get_layer_name()); @@ -220,8 +218,8 @@ void InflationFilter::update(::easynav::NavState & nav_state) dst->mark_all_changed(); df_map_ = &nm; df_geometry_ = nm.geometry_id(); - last_src_ = src.get(); - last_dst_ = dst.get(); + last_src_ = src; + last_dst_ = dst; return; } 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 ef99225e..c60519fa 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 @@ -14,11 +14,13 @@ // limitations under the License. +#include #include #include #include #include #include +#include #include #include "easynav_common/Parameters.hpp" @@ -26,7 +28,10 @@ #include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" +#include "navmap_core/Combine.hpp" +#include "navmap_core/Iterators.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_core/Occupancy.hpp" #include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/ObstacleFilter.hpp" @@ -55,6 +60,31 @@ ObstacleFilter::on_initialize() node->get_parameter(plugin_name_ + ".max_height", max_height_); node->get_parameter(plugin_name_ + ".downsample_resolution", downsample_resolution_); + easynav::declare_parameter_if_absent(*node, plugin_name_ + ".memory.enabled", memory_); + easynav::declare_parameter_if_absent(*node, plugin_name_ + ".memory.clearing", clearing_); + easynav::declare_parameter_if_absent(*node, plugin_name_ + ".memory.decay_time", decay_time_); + easynav::declare_parameter_if_absent(*node, plugin_name_ + ".memory.p_hit", p_hit_); + easynav::declare_parameter_if_absent(*node, plugin_name_ + ".memory.p_miss", p_miss_); + node->get_parameter(plugin_name_ + ".memory.enabled", memory_); + node->get_parameter(plugin_name_ + ".memory.clearing", clearing_); + node->get_parameter(plugin_name_ + ".memory.decay_time", decay_time_); + node->get_parameter(plugin_name_ + ".memory.p_hit", p_hit_); + node->get_parameter(plugin_name_ + ".memory.p_miss", p_miss_); + if (!(p_hit_ > 0.5 && p_hit_ < 1.0) || !(p_miss_ > 0.0 && 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): " + "using 0.7 and 0.4", plugin_name_.c_str()); + p_hit_ = 0.7; + p_miss_ = 0.4; + } + if (!(decay_time_ >= 0.0)) {decay_time_ = 0.0;} + if (memory_) { + RCLCPP_INFO( + node->get_logger(), "[%s] obstacle memory: clearing %s, decay %s", plugin_name_.c_str(), + clearing_ ? "on" : "off", + decay_time_ > 0.0 ? (std::to_string(decay_time_) + " s").c_str() : "off"); + } + easynav::declare_parameter_if_absent( *node, plugin_name_ + ".min_height_per_meter", min_height_per_meter_); node->get_parameter(plugin_name_ + ".min_height_per_meter", min_height_per_meter_); @@ -92,9 +122,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) const auto occ = nm.layers.get("occupancy"); const std::uint64_t occ_hash = occ && occ->size() == N ? occ->content_hash() : 0; auto layer = nm.layers.get(get_layer_name()); - if (!layer || layer.get() != last_layer_ || layer->size() != N || occ_hash != last_occ_hash_ || - layer->all_changed()) - { + if (!layer || layer != last_layer_.lock() || layer->size() != N || occ_hash != last_occ_hash_) { nm.layers.remove(get_layer_name()); base_.assign(N, navmap_ros::FREE_SPACE); if (occ_hash != 0) { @@ -106,8 +134,17 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) nm.add_layer( get_layer_name(), "obstacles: static map plus what the sensors see", "", navmap_ros::FREE_SPACE)->set_data(base_); + nm.set_layer_unknown(get_layer_name(), navmap_ros::NO_INFORMATION); + // The memory belongs to the map it was built on + nm.layers.remove("obstacles_logodds"); + nm.layers.remove("obstacles_sensor"); + active_.clear(); + in_active_.assign(N, 0); + stamp_.assign(N, 0); + flag_.assign(N, 0); + last_time_ = -1.0; layer = nm.layers.get(get_layer_name()); - last_layer_ = layer.get(); + last_layer_ = layer; last_occ_hash_ = occ_hash; marked_.clear(); seen_.assign(N, 0); @@ -124,23 +161,26 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) .fuse(map_frame) .as_points(); - // The robot's position, for min_height_per_meter - Eigen::Vector2f robot_xy(0.0f, 0.0f); + // The robot's position, for min_height_per_meter and for the clearing rays + Eigen::Vector3f robot(0.0f, 0.0f, 0.0f); bool robot_known = false; - if (min_height_per_meter_ > 0.0) { + if (min_height_per_meter_ > 0.0 || (memory_ && clearing_)) { try { const auto tf = RTTFBuffer::getInstance()->lookupTransform( 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 = {static_cast(tf.transform.translation.x), + static_cast(tf.transform.translation.y), + static_cast(tf.transform.translation.z)}; robot_known = true; } catch (const tf2::TransformException & ex) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), 5000, - "[%s] No robot pose (%s): min_height_per_meter not applied", plugin_name_.c_str(), - ex.what()); + "[%s] No robot pose (%s): min_height_per_meter and clearing not applied", + plugin_name_.c_str(), ex.what()); } } + const Eigen::Vector2f robot_xy = robot.head<2>(); + std::vector> ends; // memory: every point std::optional last_surface; std::optional<::navmap::NavCelId> last_cid; @@ -182,6 +222,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) const float height = normal.dot(query - nm.positions.at(cel.v[0])); // Farther, a small attitude error lifts the ground more: a higher threshold const double distance = robot_known ? (query.head<2>() - robot_xy).norm() : 0.0; + if (memory_) {ends.emplace_back(cid, query);} if (!(height > min_height_ + min_height_per_meter_ * distance)) {continue;} if (seen_[cid] != 2) { @@ -190,6 +231,14 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) } } + if (memory_) { + for (const auto cid : marked) { + seen_[cid] = 0; + } + update_memory(nm, robot, robot_known, marked, ends); + return; + } + // Obstacles now; cells that were obstacles in the last cycle and are not now go back to the // static map. Only cells whose value changes are written (and recorded as changed). for (const auto cid : marked) { @@ -208,6 +257,128 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) marked_.swap(marked); } +void ObstacleFilter::update_memory( + ::navmap::NavMap & nm, const Eigen::Vector3f & robot, bool robot_known, + const std::vector<::navmap::NavCelId> & hits, + const std::vector> & ends) +{ + using ::navmap::NavCelId; + using LayerU8 = ::navmap::LayerView; + const std::size_t N = nm.navcels.size(); + ::navmap::OccupancyParams op; + op.p_hit = static_cast(p_hit_); + op.p_miss = static_cast(p_miss_); + ::navmap::OccupancyLayer occ(nm, "obstacles_logodds", op); // created (unknown) if absent + if (!nm.has_layer("obstacles_sensor")) { + nm.add_layer( + "obstacles_sensor", "what the sensors remember: obstacle, free or unknown", "", + navmap_ros::NO_INFORMATION); + nm.set_layer_unknown("obstacles_sensor", navmap_ros::NO_INFORMATION); + } + auto sensor = std::dynamic_pointer_cast(nm.layers.get("obstacles_sensor")); + auto & logodds = occ.view()->data(); + if (++cycle_ == 0) { + std::fill(stamp_.begin(), stamp_.end(), 0); + cycle_ = 1; + } + std::vector touched; + auto note = [&](NavCelId c) { + if (stamp_[c] != cycle_) { + stamp_[c] = cycle_; + touched.push_back(c); + } + }; + + // Evidence of this cycle, once per cell: occupied where a point rises above the surface, free + // where a ray from the robot crosses the surface or ends on it. A hit beats a miss. + for (const auto c : hits) { + flag_[c] = 1; + note(c); + } + if (clearing_ && robot_known && !ends.empty()) { + std::size_t s; + NavCelId rc; + Eigen::Vector3f bary, hit; + ::navmap::NavMap::LocateOpts opts; + opts.use_downward_ray = true; + opts.height_eps = 0.5f; + if (nm.locate_navcel(robot + Eigen::Vector3f(0, 0, 0.1f), s, rc, bary, &hit, opts)) { + for (const auto & [end, p] : ends) { + ::navmap::for_each_on_segment( + nm, rc, robot, p, [&](NavCelId x, float, float) { + if (flag_[x] == 1) {return x != end;} // stop at the obstacle it hits + note(x); + return true; + }); + } + } + } + for (const auto c : touched) { + if (flag_[c] == 1) {occ.hit(c);} else {occ.miss(c);} + flag_[c] = 0; + } + + // Cells not observed fade back to unknown + const double now = get_node()->now().seconds(); + if (decay_time_ > 0.0 && last_time_ >= 0.0 && now > last_time_) { + const double full = ::navmap::OccupancyLayer::to_fixed( + ::navmap::OccupancyLayer::logodds_of(op.p_max)); + decay_acc_ += full * (now - last_time_) / decay_time_; + const int step = static_cast(decay_acc_); + decay_acc_ -= step; + if (step > 0) { + for (const auto c : active_) { + if (stamp_[c] == cycle_) {continue;} + const int v = logodds[c]; + const int nv = v > 0 ? std::max(0, v - step) : std::min(0, v + step); + if (nv != v) { + occ.add(c, static_cast(nv - v)); + note(c); + } + } + } + } + last_time_ = now; + std::size_t k = 0; + for (const auto c : active_) { + if (logodds[c] != 0) {active_[k++] = c;} else {in_active_[c] = 0;} + } + active_.resize(k); + for (const auto c : touched) { + if (logodds[c] != 0 && !in_active_[c]) { + in_active_[c] = 1; + active_.push_back(c); + } + } + + // What the sensors say, and the layer recomposed where it changed: the static map, and on top + // the sensors (max; they fill in unknown static cells, and never erase a static obstacle) + std::vector changed; + for (const auto c : touched) { + const auto st = occ.state(c); + const uint8_t v = st == ::navmap::OccupancyState::OCCUPIED ? navmap_ros::LETHAL_OBSTACLE : + st == ::navmap::OccupancyState::FREE ? navmap_ros::FREE_SPACE : navmap_ros::NO_INFORMATION; + if ((*sensor)[c] != v) { + (*sensor)[c] = v; + sensor->mark_changed(c); + changed.push_back(c); + } + } + if (changed.empty()) {return;} + auto layer = std::dynamic_pointer_cast(nm.layers.get(get_layer_name())); + std::vector before(changed.size()); + for (std::size_t i = 0; i < changed.size(); ++i) { + before[i] = (*layer)[changed[i]]; + (*layer)[changed[i]] = base_[changed[i]]; + } + ::navmap::CombineOptions co; + co.cells = changed; + ::navmap::combine_layer(nm, get_layer_name(), "obstacles_sensor", ::navmap::CombineOp::MAX, co); + for (std::size_t i = 0; i < changed.size(); ++i) { + if ((*layer)[changed[i]] != before[i]) {layer->mark_changed(changed[i]);} + } + (void)N; +} } // namespace navmap } // namespace easynav diff --git a/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt index efc3ca96..15c9394c 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt @@ -18,3 +18,6 @@ target_link_libraries(navmap_local_mode_tests ${PROJECT_NAME}) ament_add_gtest(navmap_incremental_filters_tests navmap_incremental_filters_tests.cpp) target_link_libraries(navmap_incremental_filters_tests ${PROJECT_NAME}) + +ament_add_gtest(navmap_obstacle_memory_tests navmap_obstacle_memory_tests.cpp) +target_link_libraries(navmap_obstacle_memory_tests ${PROJECT_NAME}) diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_memory_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_memory_tests.cpp new file mode 100644 index 00000000..8ab5aa4b --- /dev/null +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_memory_tests.cpp @@ -0,0 +1,247 @@ +// 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. + +// ObstacleFilter with memory: obstacles persist in log-odds, rays clear them, decay fades them. + +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_navmap_maps_manager/filters/ObstacleFilter.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using ::navmap::NavCelId; +using ::navmap::NavMap; + +namespace +{ + +constexpr uint8_t LETHAL = navmap_ros::LETHAL_OBSTACLE; +constexpr uint8_t FREE = navmap_ros::FREE_SPACE; +constexpr uint8_t UNKNOWN = navmap_ros::NO_INFORMATION; + +// 4 x 4 m at 0.1 m, free, with a static obstacle at (3.55, 2.05) and an unknown strip at x in +// [3.0, 3.3], y in [1.0, 1.2] +nav_msgs::msg::OccupancyGrid grid() +{ + nav_msgs::msg::OccupancyGrid g; + g.header.frame_id = "map"; + g.info.resolution = 0.1; + g.info.width = 40; + g.info.height = 40; + g.info.origin.orientation.w = 1.0; + g.data.assign(40 * 40, 0); + g.data[20 * 40 + 35] = 100; + for (int i = 30; i < 33; ++i) { + for (int j = 10; j < 12; ++j) { + g.data[j * 40 + i] = -1; + } + } + return g; +} + +class ObstacleMemoryTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + static int count = 0; + node_ = std::make_shared( + "obstacle_memory_" + std::to_string(count++)); + auto tf_buffer = easynav::RTTFBuffer::getInstance(); + geometry_msgs::msg::TransformStamped tf; + tf.header.frame_id = tf_buffer->get_tf_info().map_frame; + tf.child_frame_id = tf_buffer->get_tf_info().robot_frame; + tf.transform.translation.x = 2.0; + tf.transform.translation.y = 2.0; + tf.transform.rotation.w = 1.0; + tf_buffer->setTransform(tf, "test", true); + nav_state_.set("map.navmap", navmap_ros::from_occupancy_grid(grid())); + } + + void make(bool memory, double decay = 0.0, bool clearing = true) + { + node_->declare_parameter("navmap.obstacles.memory.enabled", memory); + node_->declare_parameter("navmap.obstacles.memory.decay_time", decay); + node_->declare_parameter("navmap.obstacles.memory.clearing", clearing); + filter_.initialize(node_, "navmap.obstacles"); + } + + // One cycle: 0.5 m columns at `cols` and ground points at `ground` (cell centers, map frame). + // The changes are cleared afterwards, as the maps manager does. + const NavMap & cycle( + const std::vector> & cols, + const std::vector> & ground = {}) + { + easynav::PointPerception p; + p.frame_id = "map"; + p.stamp = node_->now(); + p.valid = true; + for (const auto & [x, y] : cols) { + for (float z = 0.2f; z <= 0.5f; z += 0.1f) {p.data.push_back(pcl::PointXYZ(x, y, z));} + } + for (const auto & [x, y] : ground) {p.data.push_back(pcl::PointXYZ(x, y, 0.0f));} + nav_state_.set("scan", p); + filter_.update(nav_state_); + const auto & nm = nav_state_.get("map.navmap"); + last_changed_ = nm.has_layer("obstacles") ? nm.layers.get("obstacles")->changed().size() : 0; + for (const auto & n : nm.list_layers()) {nm.layers.get(n)->clear_changes();} + return nm; + } + + uint8_t at(float x, float y) + { + const auto & nm = nav_state_.get("map.navmap"); + std::size_t s; + NavCelId c; + Eigen::Vector3f b, h; + EXPECT_TRUE(nm.locate_navcel(Eigen::Vector3f(x, y, 0.05f), s, c, b, &h)); + return nm.layer_get("obstacles", c, 123); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + easynav::NavState nav_state_; + easynav::navmap::ObstacleFilter filter_; + std::size_t last_changed_ {0}; +}; + +} // namespace + +TEST_F(ObstacleMemoryTest, OffByDefaultNothingIsRemembered) +{ + filter_.initialize(node_, "navmap.obstacles"); + cycle({{3.03f, 2.03f}}); + EXPECT_EQ(at(3.03f, 2.03f), LETHAL); + const auto & nm = cycle({}); + EXPECT_EQ(at(3.03f, 2.03f), FREE); + EXPECT_FALSE(nm.has_layer("obstacles_logodds")); + EXPECT_FALSE(nm.has_layer("obstacles_sensor")); + EXPECT_EQ(nm.get_layer_meta("obstacles")->unknown, 255.0); // NO_INFORMATION is unknown +} + +TEST_F(ObstacleMemoryTest, AnObstacleOutOfSightIsRemembered) +{ + make(true); + const auto & nm = cycle({{3.03f, 2.03f}}); + EXPECT_EQ(at(3.03f, 2.03f), LETHAL); + EXPECT_TRUE(nm.has_layer("obstacles_logodds")); + EXPECT_EQ(nm.layer_type_name("obstacles_logodds"), "int16"); + for (int i = 0; i < 5; ++i) { + cycle({}); + } // nothing seen, no ray crosses it + EXPECT_EQ(at(3.03f, 2.03f), LETHAL); + EXPECT_EQ(at(1.03f, 1.03f), FREE); + EXPECT_EQ(at(3.55f, 2.05f), LETHAL); // static +} + +TEST_F(ObstacleMemoryTest, RaysThroughAnObstacleClearItInAFewCycles) +{ + make(true); + cycle({{3.03f, 2.03f}}); + // The ground is now seen beyond it, on the same ray from the robot: it was not there anymore + int cycles = 0; + while (at(3.03f, 2.03f) == LETHAL && cycles < 10) { + cycle({}, {{3.53f, 2.03f}}); + ++cycles; + } + EXPECT_EQ(at(3.03f, 2.03f), FREE); + EXPECT_GE(cycles, 2); // one miss does not undo a hit + EXPECT_LE(cycles, 3); + // A cleared cell changes once: later cycles with the same evidence record no change + cycle({}, {{3.53f, 2.03f}}); + cycle({}, {{3.53f, 2.03f}}); + EXPECT_EQ(last_changed_, 0u); +} + +TEST_F(ObstacleMemoryTest, SensorsFillInUnknownButNeverEraseTheStaticMap) +{ + make(true); + // (Points off the diagonals of the grid cells: each probes one NavCel) + EXPECT_EQ((cycle({}), at(3.16f, 1.12f)), UNKNOWN); // unknown in the static map + // The ground seen on the unknown strip, and beyond the static obstacle + for (int i = 0; i < 5; ++i) { + cycle({}, {{3.86f, 2.02f}, {3.16f, 1.12f}}); + } + EXPECT_EQ(at(3.16f, 1.12f), FREE); + EXPECT_EQ(at(3.55f, 2.05f), LETHAL); // the static obstacle stays + EXPECT_EQ(at(3.86f, 2.02f), FREE); + // The sensor layer says what the sensors know + const auto & nm = nav_state_.get("map.navmap"); + EXPECT_TRUE(nm.get_layer_meta("obstacles_sensor")->unknown.has_value()); +} + +TEST_F(ObstacleMemoryTest, UnobservedCellsDecayBackToTheStaticMap) +{ + make(true, 0.3); + cycle({{3.03f, 2.03f}}); + cycle({{3.03f, 2.03f}}); + EXPECT_EQ(at(3.03f, 2.03f), LETHAL); + // Not observed for longer than the decay time + const auto t0 = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - t0 < std::chrono::milliseconds(700)) { + cycle({}); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + EXPECT_EQ(at(3.03f, 2.03f), FREE); + EXPECT_EQ(at(3.55f, 2.05f), LETHAL); + // Observed cells do not decay + for (int i = 0; i < 20; ++i) { + cycle({{1.03f, 3.03f}}); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + EXPECT_EQ(at(1.03f, 3.03f), LETHAL); +} + +TEST_F(ObstacleMemoryTest, WithoutClearingRaysChangeNothing) +{ + make(true, 0.0, false); + cycle({{3.03f, 2.03f}}); + for (int i = 0; i < 5; ++i) { + cycle({}, {{3.53f, 2.03f}}); + } + EXPECT_EQ(at(3.03f, 2.03f), LETHAL); +} + +TEST_F(ObstacleMemoryTest, ANewMapForgetsEverything) +{ + make(true); + cycle({{3.03f, 2.03f}}); + nav_state_.set("map.navmap", navmap_ros::from_occupancy_grid(grid())); + const auto & nm = cycle({}); + EXPECT_EQ(at(3.03f, 2.03f), FREE); + EXPECT_EQ(nm.layer_size("obstacles_logodds"), nm.navcels.size()); +} + +TEST_F(ObstacleMemoryTest, InvalidProbabilitiesFallBackToTheDefaults) +{ + node_->declare_parameter("navmap.obstacles.memory.p_hit", 0.3); + node_->declare_parameter("navmap.obstacles.memory.p_miss", 0.9); + make(true); + cycle({{3.03f, 2.03f}}); + EXPECT_EQ(at(3.03f, 2.03f), LETHAL); // a hit is evidence of occupied, whatever was asked +}