Save/Load gridmaps

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

Author: fmrico

easynav_gridmap_maps_builder/CMakeLists.txt

@@ -16,20 +16,27 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(easynav_common REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(grid_map_ros REQUIRED)
 find_package(grid_map_msgs REQUIRED)
-
+find_package(ament_index_cpp REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+find_package(cv_bridge REQUIRED)
 
 set(dependencies
   rclcpp
   rclcpp_lifecycle
   sensor_msgs
   std_msgs
+  std_srvs
   geometry_msgs
   easynav_common
   grid_map_ros
   grid_map_msgs
+  ament_index_cpp
+  yaml_cpp_vendor
+  cv_bridge
 )
 
 include_directories(include)

easynav_gridmap_maps_builder/include/easynav_gridmap_maps_builder/GridmapMapsBuilderNode.hpp

@@ -29,6 +29,9 @@
 
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "grid_map_msgs/msg/grid_map.hpp"
+#include "std_srvs/srv/trigger.hpp"
+
+#include "grid_map_ros/grid_map_ros.hpp"
 
 #include "easynav_common/types/Perceptions.hpp"
 
@@ -103,7 +106,15 @@ class GridmapMapsBuilderNode : public rclcpp_lifecycle::LifecycleNode
    */
   void register_handler(std::shared_ptr<PerceptionHandler> handler);
 
+protected:
+  bool save_gridmap(const std::string & filename, const grid_map::GridMap & map);
+  bool load_gridmap(const std::string & filename, grid_map::GridMap & map);
+
+  grid_map::GridMap map_;
+  std::string map_path_;
+
 private:
+
   /// Name of the sensor topic to subscribe to (e.g., point clouds).
   std::string sensor_topic_;
 
@@ -122,6 +133,11 @@ class GridmapMapsBuilderNode : public rclcpp_lifecycle::LifecycleNode
   /// Publisher for the processed grid map.
   rclcpp_lifecycle::LifecyclePublisher<grid_map_msgs::msg::GridMap>::SharedPtr pub_;
 
+  /**
+   * @brief Service for saving current map to disk.
+   */
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr savemap_srv_;
+
   /// Registered perception handlers by sensor name.
   std::map<std::string, std::shared_ptr<PerceptionHandler>> handlers_;
 };

easynav_gridmap_maps_builder/package.xml

@@ -12,11 +12,15 @@
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>geometry_msgs</depend>
   <depend>easynav_common</depend>
   <depend>grid_map_ros</depend>
   <depend>grid_map_msgs</depend>
-  
+  <depend>ament_index_cpp</depend>
+  <depend>yaml_cpp_vendor</depend>
+  <depend>cv_bridge</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

easynav_gridmap_maps_builder/src/easynav_gridmap_maps_builder/GridmapMapsBuilderNode.cpp

@@ -23,14 +23,22 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
+#include "yaml-cpp/yaml.h"
+
 #include "lifecycle_msgs/msg/transition.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 
-#include <grid_map_ros/grid_map_ros.hpp>
+#include "grid_map_ros/grid_map_ros.hpp"
+#include "grid_map_cv/grid_map_cv.hpp"
+#include "cv_bridge/cv_bridge.hpp"
+#include "opencv2/highgui/highgui.hpp"
 
 #include "easynav_gridmap_maps_builder/GridmapMapsBuilderNode.hpp"
 #include "easynav_common/types/PointPerception.hpp"
 
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "ament_index_cpp/get_package_prefix.hpp"
+
 namespace easynav
 {
 
@@ -50,9 +58,48 @@ GridmapMapsBuilderNode::GridmapMapsBuilderNode(const rclcpp::NodeOptions & optio
     declare_parameter("perception_default_frame", "map");
   }
 
+  std::string package_name, map_path_file;
+  if (!has_parameter("package")) {
+    declare_parameter("package", package_name);
+  }
+
+  if (!has_parameter("map_path_file")) {
+    declare_parameter("map_path_file", "map_path_file");
+  }
+
+//   if (package_name != "" && map_path_file != "") {
+//     std::string pkgpath;
+//     try {
+//       pkgpath = ament_index_cpp::get_package_share_directory(package_name);
+//       map_path_ = pkgpath + "/" + map_path_file;
+//     } catch(ament_index_cpp::PackageNotFoundError & ex) {
+//       return std::unexpected("Package " + package_name + " not found. Error: " + ex.what());
+//     }
+// 
+//     if (!load_gridmap(map_path_, map_)) {
+//       return std::unexpected("File [" + map_path_ + "] not found");
+//     }
+//   }
+
   pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>(
         "map_builder_gridmap/gridmap", rclcpp::QoS(1).transient_local().reliable());
 
+//   savemap_srv_ = node->create_service<std_srvs::srv::Trigger>(
+//     node->get_name() + std::string("/") + plugin_name + "/savemap",
+//     [this](
+//       const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+//       std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+//     {
+//       (void)request;
+//       if (!save_gridmap(map_path_, map_)) {
+//         response->success = false;
+//         response->message = "Failed to save map to: " + map_path_;
+//       } else {
+//         response->success = true;
+//         response->message = "Map successfully saved to: " + map_path_;
+//       }
+//     });
+// 
   register_handler(std::make_shared<PointPerceptionHandler>());
 }
 
@@ -236,4 +283,83 @@ GridmapMapsBuilderNode::register_handler(std::shared_ptr<PerceptionHandler> hand
   handlers_[handler->group()] = handler;
 }
 
+
+bool
+GridmapMapsBuilderNode::save_gridmap(const std::string & filename, const grid_map::GridMap & map)
+{
+  std::string dir = std::filesystem::path(filename).parent_path();
+  YAML::Emitter yaml;
+  yaml << YAML::BeginMap;
+  yaml << YAML::Key << "layers" << YAML::Value << YAML::BeginSeq;
+
+  for (const std::string & layer : map.getLayers()) {
+    // Compute min and max values of the layer
+    float min_val = std::numeric_limits<float>::max();
+    float max_val = std::numeric_limits<float>::lowest();
+    for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it) {
+      if (!map.isValid(*it, layer)) continue;
+      float value = map.at(layer, *it);
+      min_val = std::min(min_val, value);
+      max_val = std::max(max_val, value);
+    }
+
+    // Save layer image
+    cv::Mat image;
+    grid_map::GridMapCvConverter::toImage<unsigned char, 1>(map, layer, CV_8UC1, min_val, max_val, image);
+    std::string image_filename = layer + ".pgm";
+    std::string image_path = dir + "/" + image_filename;
+    cv::imwrite(image_path, image);
+
+    yaml << YAML::BeginMap;
+    yaml << YAML::Key << "name" << YAML::Value << layer;
+    yaml << YAML::Key << "min" << YAML::Value << min_val;
+    yaml << YAML::Key << "max" << YAML::Value << max_val;
+    yaml << YAML::EndMap;
+  }
+  yaml << YAML::EndSeq;
+  yaml << YAML::Key << "resolution" << YAML::Value << map.getResolution();
+  yaml << YAML::Key << "length" << YAML::Value << YAML::Flow << std::vector<double>{map.getLength().x(), map.getLength().y()};
+  yaml << YAML::Key << "position" << YAML::Value << YAML::Flow << std::vector<double>{map.getPosition().x(), map.getPosition().y()};
+  yaml << YAML::EndMap;
+
+  std::ofstream file_out(filename);
+  if (!file_out) return false;
+  file_out << yaml.c_str();
+  return true;
+}
+
+bool
+GridmapMapsBuilderNode::load_gridmap(const std::string & filename, grid_map::GridMap & map)
+{
+  std::ifstream file_in(filename);
+  if (!file_in) return false;
+
+  YAML::Node config = YAML::LoadFile(filename);
+  if (!config["layers"]) return false;
+
+  map.setFrameId("map");
+
+  auto resolution = config["resolution"].as<double>();
+  auto length = config["length"].as<std::vector<double>>();
+  auto position = config["position"].as<std::vector<double>>();
+
+  map.setGeometry(grid_map::Length(length[0], length[1]), resolution, grid_map::Position(position[0], position[1]));
+
+  std::string dir = std::filesystem::path(filename).parent_path();
+
+  for (const auto & layer_node : config["layers"]) {
+    std::string layer_name = layer_node["name"].as<std::string>();
+    float min_val = layer_node["min"].as<float>();
+    float max_val = layer_node["max"].as<float>();
+
+    std::string image_path = dir + "/" + layer_name + ".pgm";
+    cv::Mat image = cv::imread(image_path, cv::IMREAD_GRAYSCALE);
+    if (image.empty()) return false;
+
+    grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(image, layer_name, map, min_val, max_val);
+  }
+
+  return true;
+}
+
 } // namespace easynav

easynav_gridmap_maps_builder/tests/CMakeLists.txt

@@ -8,4 +8,12 @@ target_link_libraries(gridmap_maps_builder_tests
 )
 ament_target_dependencies(gridmap_maps_builder_tests ${dependencies})
 
-
+ament_add_gtest(gridmap_maps_load_save_tests gridmap_maps_load_save_tests.cpp)
+target_include_directories(gridmap_maps_load_save_tests PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(gridmap_maps_load_save_tests
+  ${PROJECT_NAME}
+)
+ament_target_dependencies(gridmap_maps_load_save_tests ${dependencies})