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

@@ -15,14 +15,17 @@ Affiliation: [ETH Zurich, Autonomous Systems Lab](https://asl.ethz.ch/)<br />**
 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 --packages-up-to grid_map_geo
 ```
@@ -65,8 +68,22 @@ ros2 launch grid_map_geo load_tif_launch.xml
 ## Running the package
 
 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

@@ -97,6 +97,14 @@ class GridMapGeo {
    */
   bool Load(const std::string& map_path) { Load(map_path, ""); }
 
+  /**
+   * @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
    *
@@ -183,5 +191,10 @@ class GridMapGeo {
   Location maporigin_;
   std::string frame_id_{""};
   std::string coordinate_name_{""};
+
+ 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

@@ -62,6 +62,15 @@ GridMapGeo::GridMapGeo(const std::string &frame_id) { frame_id_ = frame_id; }
 
 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, const std::string color_map_path) {
   bool loaded = initializeFromGdalDataset(map_path);
   if (!color_map_path.empty()) {  // Load color layer if the color path is nonempty
@@ -106,20 +115,30 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
   coordinate_name_ = spatial_ref->GetAttrValue("geogcs");
 
   // 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);
 
@@ -128,20 +147,26 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
   grid_map_.setGeometry(length, resolution, position);
   grid_map_.setFrameId(frame_id_);
   grid_map_.add("elevation");
-  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);
+  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);
 
-  // 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);
+  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) + width * gridMapIndex(1)];
-  // }
+    layer_elevation(x, y) = data[gridMapIndex(0) + grid_width * gridMapIndex(1)];
+  }
 
   return true;
 }
@@ -171,7 +196,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
@@ -232,7 +257,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

@@ -37,6 +37,9 @@
  * @author Jaeyoung Lim <[email protected]>
  */
 
+#include <algorithm>
+#include <limits>
+
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "grid_map_geo/grid_map_geo.hpp"
 #include "grid_map_msgs/msg/grid_map.h"
@@ -50,18 +53,36 @@ using namespace std::chrono_literals;
 class MapPublisher : public rclcpp::Node {
  public:
   MapPublisher() : Node("map_publisher") {
-    std::string file_path = this->declare_parameter("tif_path", ".");
-    std::string color_path = this->declare_parameter("tif_color_path", ".");
-    std::string frame_id = this->declare_parameter("frame_id", "map");
-
     original_map_pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", 1);
+    const std::string frame_id = this->declare_parameter("frame_id", "map");
+    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(), "file_path " << file_path);
+    RCLCPP_INFO_STREAM(get_logger(), "file_path " << gdal_dataset_path);
     RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path);
     tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
 
     map_ = std::make_shared<GridMapGeo>(frame_id);
-    map_->Load(file_path, color_path);
+    map_->setMaxMapSizePixels(max_map_width, max_map_height);
+    map_->Load(gdal_dataset_path, color_path);
     auto timer_callback = [this]() -> void {
       auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap());
       if (msg) {