Add VRT support

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

Author: Ryanf55

CMakeLists.txt

@@ -51,14 +51,18 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC
   grid_map_ros
 )
 
-add_executable(test_tif_loader
-  src/test_tif_loader.cpp
+add_executable(map_publisher
+  src/map_publisher.cpp
 )
 
-target_link_libraries(test_tif_loader PUBLIC
+target_link_libraries(map_publisher PUBLIC
   ${PROJECT_NAME}
 )
 
+add_executable(vsicurl_local_cache
+  src/gdal_vsicurl_local_cache.cpp
+)
+
 # Install
 install(
   DIRECTORY include/
@@ -79,7 +83,7 @@ ament_export_dependencies(GDAL)
 
 install(
   TARGETS
-  test_tif_loader
+  map_publisher vsicurl_local_cache
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -88,6 +92,11 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+install(DIRECTORY
+  resources
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 install(DIRECTORY
   rviz
   DESTINATION share/${PROJECT_NAME}/

README.md

@@ -9,7 +9,7 @@ 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 GeoTIFF files.
+Install the dependencies. This package depends on gdal, to read georeferenced images and DEM files.
 
 Pull in dependencies using rosdep
 ```

include/grid_map_geo/grid_map_geo.hpp

@@ -39,19 +39,6 @@
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
 
-#if __APPLE__
-#include <cpl_string.h>
-#include <gdal.h>
-#include <gdal_priv.h>
-#include <ogr_p.h>
-#include <ogr_spatialref.h>
-#else
-#include <gdal/cpl_string.h>
-#include <gdal/gdal.h>
-#include <gdal/gdal_priv.h>
-#include <gdal/ogr_p.h>
-#include <gdal/ogr_spatialref.h>
-#endif
 
 #include <iostream>
 struct Location {
@@ -116,14 +103,14 @@ class GridMapGeo {
   bool Load(const std::string& map_path, bool algin_terrain, const std::string color_map_path = "");
 
   /**
-   * @brief Initialize grid map from a geotiff file
+   * @brief Initialize grid map from a GDAL dataset
    *
-   * @param path Path to dsm path (Supported formats are *.tif)
+   * @param path Path to dsm path (Supported formats are https://gdal.org/drivers/raster/index.html)
    * @param align_terrain
    * @return true Successfully loaded terrain
    * @return false Failed to load terrain
    */
-  bool initializeFromGeotiff(const std::string& path, bool align_terrain = true);
+  bool initializeFromGdalDataset(const std::string& path, bool align_terrain = true);
 
   /**
    * @brief Load a color layer from a geotiff file (orthomosaic)

launch/load_tif.launch.py

@@ -32,7 +32,7 @@ def generate_launch_description():
     tif_loader = Node(
         package="grid_map_geo",
         namespace="grid_map_geo",
-        executable="test_tif_loader",
+        executable="map_publisher",
         name="tif_loader",
         parameters=[
             {"tif_path": LaunchConfiguration("tif_path")},

launch/load_tif_launch.xml

@@ -3,7 +3,7 @@
     <arg name="location" default="sargans"/>
     <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="test_tif_loader" name="test_tif_loader" output="screen">
+    <node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" output="screen">
         <param name="tif_path" value="$(find-pkg-share grid_map_geo)/resources/sargans.tif"/>
         <param name="tif_color_path" value="$(find-pkg-share grid_map_geo)/resources/sargans_color.tif"/>
     </node>

launch/load_vrt_launch.xml

@@ -0,0 +1,12 @@
+<launch>
+    <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">
+        <param name="gdal_dataset_path" value="$(find-pkg-share grid_map_geo)/resources/ap_srtm1.vrt"/>
+    </node>
+
+    <group if="$(var rviz)">
+        <node exec="rviz2" name="rviz2" pkg="rviz2"  args="-d $(find-pkg-share grid_map_geo)/launch/config.rviz" />
+    </group>
+</launch>

resources/ap_srtm1.vrt

@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 Jaeyoung Lim, ASL, ETH Zurich, Switzerland
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name terrain-navigation nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @brief Support caching VSICURL requests to local storage to reduce network usage
+ *
+ * @author Ryan Friedman <[email protected]>
+ */
+
+
+int main() {
+
+    return 0;
+}

src/gdal_vsicurl_local_cache.cpp

@@ -43,27 +43,41 @@
 #include <grid_map_core/iterators/CircleIterator.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
 
+#if __APPLE__
+#include <cpl_string.h>
+#include <gdal.h>
+#include <gdal_priv.h>
+#include <ogr_p.h>
+#include <ogr_spatialref.h>
+#else
+#include <gdal/cpl_string.h>
+#include <gdal/gdal.h>
+#include <gdal/gdal_priv.h>
+#include <gdal/ogr_p.h>
+#include <gdal/ogr_spatialref.h>
+#endif
+
 GridMapGeo::GridMapGeo() {}
 
 GridMapGeo::~GridMapGeo() {}
 
 bool GridMapGeo::Load(const std::string &map_path, bool algin_terrain, const std::string color_map_path) {
-  bool loaded = initializeFromGeotiff(map_path, algin_terrain);
+  bool loaded = initializeFromGdalDataset(map_path, algin_terrain);
   if (!color_map_path.empty()) {  // Load color layer if the color path is nonempty
     bool color_loaded = addColorFromGeotiff(color_map_path);
   }
   if (!loaded) return false;
   return true;
 }
 
-bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terrain) {
+bool GridMapGeo::initializeFromGdalDataset(const std::string &path, bool align_terrain) {
   GDALAllRegister();
   GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly);
   if (!dataset) {
     std::cout << "Failed to open" << std::endl;
     return false;
   }
-  std::cout << std::endl << "Loading GeoTIFF file for gridmap" << std::endl;
+  std::cout << std::endl << "Loading GDAL Dataset for gridmap" << std::endl;
 
   double originX, originY, pixelSizeX, pixelSizeY;
   double geoTransform[6];
@@ -80,17 +94,24 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra
   const char *pszProjection = dataset->GetProjectionRef();
   std::cout << std::endl << "Wkt ProjectionRef: " << pszProjection << std::endl;
 
+  if (strlen(pszProjection) == 0) {
+    // https://gdal.org/user/raster_data_model.html#coordinate-system
+    std::cerr << std::endl << "Wkt ProjectionRef is unknown!" << std::endl;
+    return false;
+  }
+
   // Get image metadata
   unsigned width = dataset->GetRasterXSize();
   unsigned height = dataset->GetRasterYSize();
   double resolution = pixelSizeX;
-  std::cout << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
+  std::cout <<  __LINE__  << "RasterX: " << width << " RasterY: " << height << " pixelSizeX: " << pixelSizeX << 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
   const double lengthX = resolution * width;
   const double lengthY = resolution * height;
   grid_map::Length length(lengthX, lengthY);
+  std::cout <<  __LINE__  << "GMLX: " << lengthX << " GMLY: " << lengthY << std::endl;
 
   double mapcenter_e = originX + pixelSizeX * width * 0.5;
   double mapcenter_n = originY + pixelSizeY * height * 0.5;
@@ -115,36 +136,39 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra
   // }
 
   grid_map_.setGeometry(length, resolution, position);
-  /// TODO: Use TF for geocoordinates
-  grid_map_.setFrameId("map");
-  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);
-
-  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");
+  // 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: 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));
-  }
+  // /// 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;
+  return false;
 }
 
 bool GridMapGeo::addColorFromGeotiff(const std::string &path) {
@@ -172,7 +196,7 @@ bool GridMapGeo::addColorFromGeotiff(const std::string &path) {
   unsigned width = dataset->GetRasterXSize();
   unsigned height = dataset->GetRasterYSize();
   double resolution = pixelSizeX;
-  std::cout << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
+  std::cout <<  __LINE__  << "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
@@ -233,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 << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
+  std::cout << __LINE__ << "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/grid_map_geo.cpp

@@ -52,14 +52,14 @@ class MapPublisher : public rclcpp::Node {
     MapPublisher() : Node("map_publisher") {
       original_map_pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", 1);
 
-      std::string file_path = this->declare_parameter("tif_path", ".");
+      std::string gdal_dataset_path = this->declare_parameter("gdal_dataset_path", ".");
       std::string color_path = this->declare_parameter("tif_color_path", ".");
 
-      RCLCPP_INFO_STREAM(get_logger(), "file_path " << file_path);
-      RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path);
+      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_->Load(file_path, false, color_path);
+      map_->Load(gdal_dataset_path, false, color_path);
       timer_ = this->create_wall_timer(5s, std::bind(&MapPublisher::timer_callback, this));
     }
   private: