NavMap ObstacleFilter: optional obstacle memory

maps_managers/easynav_navmap_maps_manager/README.md

@@ -68,7 +68,8 @@ With `<plugin>.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 `<plugin>.local.enabled`, the maps manager keeps a NavMap around the robot,
 | `<plugin>.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. |
 | `<plugin>.max_height` | `double` | `NaN` | Points above this height (robot frame) are ignored; `NaN` disables it. |
 | `<plugin>.downsample_resolution` | `double` | `0.3` | Voxel size (m) the points are reduced to first; `<= 0` disables it. |
+| `<plugin>.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. |
+| `<plugin>.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. |
+| `<plugin>.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. |
+| `<plugin>.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"`. |
 

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<char> lethal_;  // obstacle cells the distance field is seeded with
 };
 

maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp

@@ -19,7 +19,9 @@
 
 #include <cstdint>
 #include <limits>
+#include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 #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<std::pair<::navmap::NavCelId, Eigen::Vector3f>> & 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<uint8_t> in_active_, flag_;
+  std::vector<std::uint32_t> 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<uint8_t> base_;                 // static value of each cell
   std::vector<::navmap::NavCelId> marked_;    // obstacle cells of the last cycle

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<float>(cost_scaling_factor_);
   auto dst = std::dynamic_pointer_cast<LayerU8>(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;
   }
 

maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp

@@ -14,19 +14,24 @@
 // limitations under the License.
 
 
+#include <algorithm>
 #include <cmath>
 #include <cstdint>
 #include <memory>
 #include <optional>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "easynav_common/Parameters.hpp"
 #include "easynav_common/types/NavState.hpp"
 #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<uint8_t>(
       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<float>(tf.transform.translation.x),
-        static_cast<float>(tf.transform.translation.y)};
+      robot = {static_cast<float>(tf.transform.translation.x),
+        static_cast<float>(tf.transform.translation.y),
+        static_cast<float>(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<std::pair<::navmap::NavCelId, Eigen::Vector3f>> ends;  // memory: every point
 
   std::optional<size_t> 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<std::pair<::navmap::NavCelId, Eigen::Vector3f>> & ends)
+{
+  using ::navmap::NavCelId;
+  using LayerU8 = ::navmap::LayerView<uint8_t>;
+  const std::size_t N = nm.navcels.size();
+  ::navmap::OccupancyParams op;
+  op.p_hit = static_cast<float>(p_hit_);
+  op.p_miss = static_cast<float>(p_miss_);
+  ::navmap::OccupancyLayer occ(nm, "obstacles_logodds", op);  // created (unknown) if absent
+  if (!nm.has_layer("obstacles_sensor")) {
+    nm.add_layer<uint8_t>(
+      "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<LayerU8>(nm.layers.get("obstacles_sensor"));
+  auto & logodds = occ.view()->data();
+  if (++cycle_ == 0) {
+    std::fill(stamp_.begin(), stamp_.end(), 0);
+    cycle_ = 1;
+  }
+  std::vector<NavCelId> 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<int>(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<int16_t>(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<NavCelId> 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<LayerU8>(nm.layers.get(get_layer_name()));
+  std::vector<uint8_t> 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

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})