NavMap maps manager: local NavMap around the robot

maps_managers/easynav_navmap_maps_manager/README.md

@@ -44,6 +44,21 @@ All parameters are declared under the plugin namespace, i.e.
 | `<plugin>.pcd_path_file` | `string` | `""` | Relative path (inside the package) to a point cloud (PCD/PLY) used to build a NavMap surface. |
 | `<plugin>.filters` | `list<string>` | `[]` | Ordered list of filter plugin names to be applied after map loading (e.g. `["obstacles", "inflation"]`). |
 
+### Local mode
+
+With `<plugin>.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 `<plugin>/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 |
+|---|---|---:|---|
+| `<plugin>.local.enabled` | `bool` | `false` | Enable the local mode. |
+| `<plugin>.local.frame` | `string` | robot frame | Frame of the NavMap (e.g. `base_footprint`, ground at z = 0). |
+| `<plugin>.local.range` | `double` | `10.0` | Half side (m) of the square window around the robot. |
+| `<plugin>.local.min_z`, `<plugin>.local.max_z` | `double` | `-3.0`, `3.0` | Height band (m, in `frame`) of the points used. |
+| `<plugin>.local.accumulate_time` | `double` | `1.0` | Scans put together before meshing (s): a single 3D scan is sparse rings. |
+| `<plugin>.local.freq` | `double` | `2.0` | Rebuilds per second (`<= 0`: every update). |
+| `<plugin>.local.downsample` | `double` | `0.1` | Voxel (m) the accumulated points are reduced to. |
+| `<plugin>.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**

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 <deque>
 #include <vector>
 #include <memory>
 #include <string>
@@ -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<std::shared_ptr<NavMapFilter>> 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<pcl::PointXYZ> cloud;  ///< In the odom frame
+  };
+  std::deque<TimedCloud> 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

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 <cmath>
 #include <stdexcept>
 #include <string>
+#include <unordered_set>
 
 #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<float>(res);
+    b.max_dz = static_cast<float>(max_dz);
+    b.max_slope_deg = static_cast<float>(slope);
+    b.min_clearance = static_cast<float>(clearance);
+    b.min_surface_area = static_cast<float>(min_area);
+    b.max_hole_area = static_cast<float>(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<std::string> 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<PointPerception>();
+    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<float>(q.w), static_cast<float>(q.x), static_cast<float>(q.y),
+    static_cast<float>(q.z));
+  const Eigen::Vector3f tr(
+    static_cast<float>(t.x), static_cast<float>(t.y), static_cast<float>(t.z));
+  const float range = static_cast<float>(local_.range);
+  const float vox = static_cast<float>(std::max(local_.downsample, 1e-3));
+
+  pcl::PointCloud<pcl::PointXYZ> local;
+  std::unordered_set<std::int64_t> 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::int64_t>(std::floor(v.x() / vox)) * 73856093) ^
+        (static_cast<std::int64_t>(std::floor(v.y() / vox)) * 19349663) ^
+        (static_cast<std::int64_t>(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 <pluginlib/class_list_macros.hpp>

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_,

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<float>(tf.transform.translation.x),
         static_cast<float>(tf.transform.translation.y)};
       robot_known = true;

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