Add limits for max map size

* And instructions for debugging with gdb
* GDAL segfaults in RasterIo() with too large of a dataset

Signed-off-by: Ryan Friedman <[email protected]>

Author: Ryanf55

README.md

@@ -1,4 +1,5 @@
 # grid_map_geo
+
 [![Build Test](https://github.com/ethz-asl/grid_map_geo/actions/workflows/build_test.yml/badge.svg)](https://github.com/ethz-asl/grid_map_geo/actions/workflows/build_test.yml)
 
 This package provides a georeferenced extension to the elevation map [grid_map](https://github.com/ANYbotics/grid_map) using [GDAL](https://gdal.org/), library for raster and vector geospatial data formats
@@ -8,24 +9,45 @@ This package provides a georeferenced extension to the elevation map [grid_map](
 
 **Authors: Jaeyoung Lim<br />
 Affiliation: [ETH Zurich, Autonomous Systems Lab](https://asl.ethz.ch/)<br />**
+
 ## Setup
+
 Install the dependencies. This package depends on gdal, to read georeferenced images and DEM files.
 
 Pull in dependencies using rosdep
-```
+```bash
 source /opt/ros/humble/setup.bash
 rosdep update
 # Assuming the package is cloned in the src folder of a ROS workspace...
 rosdep install --from-paths src --ignore-src -y
 ```
 
 Build the package
+
+```bash
+colcon build --mixin release --packages-up-to grid_map_geo
 ```
-colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False --packages-up-to grid_map_geo
-```
+
 ## Running the package
-The default launch file can be run as the following command. 
-```
+
+The default launch file can be run as the following command.
+
+```bash
 source install/setup.bash
 ros2 launch grid_map_geo load_tif_launch.xml
 ```
+
+To debug the map publisher in GDB:
+
+```bash
+colcon build --mixin debug --packages-up-to grid_map_geo --symlink-install
+source install/setup.bash
+
+# To debug the node under GDB
+ros2 run --prefix 'gdb -ex run --args' \
+grid_map_geo map_publisher --ros-args \
+-p gdal_dataset_path:=install/grid_map_geo/share/grid_map_geo/resources/ap_srtm1.vrt
+
+# To debug from the launch file
+ros2 launch grid_map_geo load_vrt_launch.xml
+```

include/grid_map_geo/grid_map_geo.hpp

@@ -91,6 +91,14 @@ class GridMapGeo {
    */
   void setAltitudeOrigin(const double altitude) { localorigin_altitude_ = altitude; };
 
+  /**
+   * @brief Helper function for setting maximum map size. Set to 0 to disable bounds check.
+   *
+   * @param pixels_x Maximum number of raster pixels in the X direction
+   * @param pixels_y Maximum number of raster pixels in the Y direction
+   */
+  void setMaxMapSizePixels(const int pixels_x, const int pixels_y);
+
   /**
    * @brief Helper function for loading terrain from path
    *
@@ -178,5 +186,12 @@ class GridMapGeo {
   double localorigin_n_{177416.56};
   double localorigin_altitude_{0.0};
   Location maporigin_;
+
+ private:
+
+  // Set default map size occupying 4MB RAM assuming 32 bit height precision.
+  int max_raster_x_size_ {1024};
+  int max_raster_y_size_ {1024};
+
 };
 #endif

launch/load_vrt_launch.xml

@@ -2,7 +2,7 @@
     <arg name="rviz" default="false"/>
     <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
 
-    <node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" output="screen">
+    <node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" output="screen" launch-prefix="gdb -ex run --args">
         <param name="gdal_dataset_path" value="$(find-pkg-share grid_map_geo)/resources/ap_srtm1.vrt"/>
     </node>
 

src/grid_map_geo.cpp

@@ -61,6 +61,17 @@ GridMapGeo::GridMapGeo() {}
 
 GridMapGeo::~GridMapGeo() {}
 
+
+void GridMapGeo::setMaxMapSizePixels(const int pixels_x, const int pixels_y) {
+  // Use int type to match GDAL , but validate it's a positive value.
+  assert(pixels_x >= 0);
+  assert(pixels_y >= 0);
+
+  max_raster_x_size_ = pixels_x;
+  max_raster_y_size_ = pixels_y;
+}
+
+
 bool GridMapGeo::Load(const std::string &map_path, bool algin_terrain, const std::string color_map_path) {
   bool loaded = initializeFromGdalDataset(map_path, algin_terrain);
   if (!color_map_path.empty()) {  // Load color layer if the color path is nonempty
@@ -101,20 +112,29 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path, bool align_t
   }
 
   // Get image metadata
-  unsigned width = dataset->GetRasterXSize();
-  unsigned height = dataset->GetRasterYSize();
+  const auto raster_width = dataset->GetRasterXSize();
+  const auto raster_height = dataset->GetRasterYSize();
+
+  if (raster_width > max_raster_x_size_ || raster_height > max_raster_y_size_) {
+    std::cout << "Raster too big. Using a submap of size " << max_raster_x_size_ << "x" << max_raster_y_size_ << std::endl;
+  }
   double resolution = pixelSizeX;
-  std::cout <<  __LINE__  << "RasterX: " << width << " RasterY: " << height << " pixelSizeX: " << pixelSizeX << std::endl;
+  std::cout << "RasterX: " << raster_width << " RasterY: " << raster_height << " pixelSizeX: " << pixelSizeX << std::endl;
+
+
+  // Limit grid map size to not exceed memory limitations
+  const auto grid_width = std::min(raster_width, max_raster_x_size_);
+  const auto grid_height = std::min(raster_height, max_raster_y_size_);
 
   // pixelSizeY is negative because the origin of the image is the north-east corner and positive
   // Y pixel coordinates go towards the south
-  const double lengthX = resolution * width;
-  const double lengthY = resolution * height;
+  const double lengthX = resolution * grid_width;
+  const double lengthY = resolution * grid_height;
   grid_map::Length length(lengthX, lengthY);
-  std::cout <<  __LINE__  << "GMLX: " << lengthX << " GMLY: " << lengthY << std::endl;
+  std::cout << "GMLX: " << lengthX << " GMLY: " << lengthY << std::endl;
 
-  double mapcenter_e = originX + pixelSizeX * width * 0.5;
-  double mapcenter_n = originY + pixelSizeY * height * 0.5;
+  double mapcenter_e = originX + pixelSizeX * grid_width * 0.5;
+  double mapcenter_n = originY + pixelSizeY * grid_height * 0.5;
   maporigin_.espg = ESPG::CH1903_LV03;
   maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
 
@@ -136,39 +156,41 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path, bool align_t
   // }
 
   grid_map_.setGeometry(length, resolution, position);
-  // /// TODO: Use TF for geocoordinates
-  // grid_map_.setFrameId("map");
-  // grid_map_.add("elevation");
-  // const auto raster_count = dataset->GetRasterCount();
-  // assert(raster_count == 2);
-  // std::cout <<  __LINE__  << "Raster Count: " << raster_count << std::endl;
-  // GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
-
-  // std::vector<float> data(width * height, 0.0f);
-  // elevationBand->RasterIO(GF_Read, 0, 0, width, height, &data[0], width, height, GDT_Float32, 0, 0);
-
-  // grid_map::Matrix &layer_elevation = grid_map_["elevation"];
-  // for (grid_map::GridMapIterator iterator(grid_map_); !iterator.isPastEnd(); ++iterator) {
-  //   const grid_map::Index gridMapIndex = *iterator;
-  //   // TODO: This may be wrong if the pixelSizeY > 0
-  //   int x = width - 1 - gridMapIndex(0);
-  //   int y = gridMapIndex(1);
-
-  //   layer_elevation(x, y) = data[gridMapIndex(0) + width * gridMapIndex(1)];
-  // }
+  /// TODO: Use TF for geocoordinates
+  grid_map_.setFrameId("map");
+  grid_map_.add("elevation");
 
-  // /// TODO: This is a workaround with the problem of gdal 3 not translating altitude correctly.
-  // /// This section just levels the current position to the ground
-  // double altitude{0.0};
-  // if (grid_map_.isInside(Eigen::Vector2d(0.0, 0.0))) {
-  //   altitude = grid_map_.atPosition("elevation", Eigen::Vector2d(0.0, 0.0));
-  // }
+  const auto raster_count = dataset->GetRasterCount();
+  assert(raster_count == 1); // expect only elevation data, otherwise it's the wrong dataset.
+  GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
 
-  // Eigen::Translation3d meshlab_translation(0.0, 0.0, -altitude);
-  // Eigen::AngleAxisd meshlab_rotation(Eigen::AngleAxisd::Identity());
-  // Eigen::Isometry3d transform = meshlab_translation * meshlab_rotation;  // Apply affine transformation.
-  // grid_map_ = grid_map_.getTransformedMap(transform, "elevation", grid_map_.getFrameId(), true);
-  return false;
+  std::vector<float> data(grid_width * grid_height, 0.0f);
+  const auto raster_err = elevationBand->RasterIO(GF_Read, 0, 0, grid_width, grid_height, &data[0], grid_width, grid_height, GDT_Float32, 0, 0);
+  if (raster_err != CPLE_None ) {
+    return false;
+  }
+  grid_map::Matrix &layer_elevation = grid_map_["elevation"];
+  for (grid_map::GridMapIterator iterator(grid_map_); !iterator.isPastEnd(); ++iterator) {
+    const grid_map::Index gridMapIndex = *iterator;
+    // TODO: This may be wrong if the pixelSizeY > 0
+    int x = grid_width - 1 - gridMapIndex(0);
+    int y = gridMapIndex(1);
+
+    layer_elevation(x, y) = data[gridMapIndex(0) + grid_width * gridMapIndex(1)];
+  }
+
+  /// TODO: This is a workaround with the problem of gdal 3 not translating altitude correctly.
+  /// This section just levels the current position to the ground
+  double altitude{0.0};
+  if (grid_map_.isInside(Eigen::Vector2d(0.0, 0.0))) {
+    altitude = grid_map_.atPosition("elevation", Eigen::Vector2d(0.0, 0.0));
+  }
+
+  Eigen::Translation3d meshlab_translation(0.0, 0.0, -altitude);
+  Eigen::AngleAxisd meshlab_rotation(Eigen::AngleAxisd::Identity());
+  Eigen::Isometry3d transform = meshlab_translation * meshlab_rotation;  // Apply affine transformation.
+  grid_map_ = grid_map_.getTransformedMap(transform, "elevation", grid_map_.getFrameId(), true);
+  return true;
 }
 
 bool GridMapGeo::addColorFromGeotiff(const std::string &path) {
@@ -196,7 +218,7 @@ bool GridMapGeo::addColorFromGeotiff(const std::string &path) {
   unsigned width = dataset->GetRasterXSize();
   unsigned height = dataset->GetRasterYSize();
   double resolution = pixelSizeX;
-  std::cout <<  __LINE__  << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
+  std::cout << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
 
   // pixelSizeY is negative because the origin of the image is the north-east corner and positive
   // Y pixel coordinates go towards the south
@@ -257,7 +279,7 @@ bool GridMapGeo::addLayerFromGeotiff(const std::string &layer_name, const std::s
   unsigned width = dataset->GetRasterXSize();
   unsigned height = dataset->GetRasterYSize();
   double resolution = pixelSizeX;
-  std::cout << __LINE__ << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
+  std::cout << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
 
   // pixelSizeY is negative because the origin of the image is the north-east corner and positive
   // Y pixel coordinates go towards the south

src/map_publisher.cpp

@@ -45,22 +45,46 @@
 #include "grid_map_msgs/msg/grid_map.h"
 #include "grid_map_ros/GridMapRosConverter.hpp"
 
+#include <algorithm>
+#include <limits>
+
 using namespace std::chrono_literals;
 
 class MapPublisher : public rclcpp::Node {
   public:
     MapPublisher() : Node("map_publisher") {
       original_map_pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", 1);
 
-      std::string gdal_dataset_path = this->declare_parameter("gdal_dataset_path", ".");
-      std::string color_path = this->declare_parameter("tif_color_path", ".");
+      const std::string gdal_dataset_path = this->declare_parameter("gdal_dataset_path", ".");
+      const std::string color_path = this->declare_parameter("tif_color_path", ".");
+      rcl_interfaces::msg::ParameterDescriptor max_map_descriptor;
+      max_map_descriptor.read_only = true;
+      max_map_descriptor.description = "Maximum number of raster pixels able to be loaded. \
+        Useful when working with large raster datasets to limit memory usage. \
+        Set to 0 to disable limits (may cause runtime crash).";
+      rcl_interfaces::msg::IntegerRange max_map_descriptor_int_range;
+
+      max_map_descriptor_int_range.from_value = 0;
+      max_map_descriptor_int_range.to_value = std::numeric_limits<int>::max();
+      max_map_descriptor.integer_range.push_back(max_map_descriptor_int_range);
+
+      const uint16_t max_map_width = std::clamp(
+        this->declare_parameter("max_map_width", 1024, max_map_descriptor),
+        max_map_descriptor_int_range.from_value,
+        max_map_descriptor_int_range.to_value);
+      const uint16_t max_map_height = std::clamp(
+        this->declare_parameter("max_map_height", 1024, max_map_descriptor),
+        max_map_descriptor_int_range.from_value,
+        max_map_descriptor_int_range.to_value);
+
 
       RCLCPP_INFO_STREAM(get_logger(), "gdal_dataset_path: '" << gdal_dataset_path << "'");
       RCLCPP_INFO_STREAM(get_logger(), "color_path: '" << color_path << "'");
 
       map_ = std::make_shared<GridMapGeo>();
+      map_->setMaxMapSizePixels(max_map_width, max_map_height);
       map_->Load(gdal_dataset_path, false, color_path);
-      timer_ = this->create_wall_timer(5s, std::bind(&MapPublisher::timer_callback, this));
+      // timer_ = this->create_wall_timer(5s, std::bind(&MapPublisher::timer_callback, this));
     }
   private:
     void timer_callback() {