Final version

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Author: fmrico

easynav_gridmap_maps_manager/easynav_gridmap_maps_manager_plugins.xml

@@ -1,6 +1,6 @@
 <class_libraries>
-  <library path="easynav_simple_maps_manager">
-    <class name="easynav_simple_maps_manager/GridmapMapsManager"  type="easynav::GridmapMapsManager" base_class_type="easynav::MapsManagerBase">
+  <library path="easynav_gridmap_maps_manager">
+    <class name="easynav_gridmap_maps_manager/GridmapMapsManager"  type="easynav::GridmapMapsManager" base_class_type="easynav::MapsManagerBase">
       <description>
       A gridmap implementation for the Maps Manager.
       </description>

easynav_gridmap_maps_manager/src/easynav_gridmap_maps_manager/GridmapMapsManager.cpp

@@ -81,6 +81,9 @@ GridmapMapsManager::on_initialize()
     if (!load_gridmap(map_path_, map_)) {
       return std::unexpected("File [" + map_path_ + "] not found or read error");
     }
+
+    RCLCPP_INFO(node->get_logger(), "Loaded gridmap from [%s] size = (%lf, %lf) \tresolution = %lf",
+      map_path_.c_str(), map_.getLength().x(), map_.getLength().y(), map_.getResolution());
   }
 
   gridmap_pub_ = node->create_publisher<grid_map_msgs::msg::GridMap>(
@@ -134,6 +137,10 @@ GridmapMapsManager::update(NavState & nav_state)
   if (map_need_update_) {
     nav_state.set("map.elevation", map_);
     map_need_update_ = false;
+
+    gridmap_msg_ = *grid_map::GridMapRosConverter::toMessage(map_);
+    gridmap_msg_.header.stamp = this->get_node()->now();
+    gridmap_pub_->publish(gridmap_msg_);
   }
 }