Add VRT support

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

Author: Ryanf55

CMakeLists.txt

@@ -53,14 +53,18 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC
   tf2_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/
@@ -81,7 +85,7 @@ ament_export_dependencies(GDAL)
 
 install(
   TARGETS
-  test_tif_loader
+  map_publisher vsicurl_local_cache
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -90,6 +94,11 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+install(DIRECTORY
+  resources
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 install(DIRECTORY
   rviz
   DESTINATION share/${PROJECT_NAME}/

README.md

@@ -12,7 +12,7 @@ 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

@@ -105,16 +105,16 @@ class GridMapGeo {
    * @return true Successfully loaded terrain
    * @return false Failed to load terrain
    */
-  bool Load(const std::string& map_path, const std::string& color_map_path);
+  bool Load(const std::string& map_path, 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)
    * @return true Successfully loaded terrain
    * @return false Failed to load terrain
    */
-  bool initializeFromGeotiff(const std::string& path);
+  bool initializeFromGdalDataset(const std::string& path);
 
   /**
    * @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" namespace="grid_map_geo" output="screen">
+    <node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" namespace="grid_map_geo" 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,40 @@
+/****************************************************************************
+ *
+ *   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

@@ -62,23 +62,23 @@ GridMapGeo::GridMapGeo(const std::string &frame_id) { frame_id_ = frame_id; }
 
 GridMapGeo::~GridMapGeo() {}
 
-bool GridMapGeo::Load(const std::string &map_path, const std::string &color_map_path) {
-  bool loaded = initializeFromGeotiff(map_path);
+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
     bool color_loaded = addColorFromGeotiff(color_map_path);
   }
   if (!loaded) return false;
   return true;
 }
 
-bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
+bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
   GDALAllRegister();
   const auto dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(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;
   std::array<double, 6> geoTransform;
@@ -104,17 +104,19 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
 
   const OGRSpatialReference *spatial_ref = dataset->GetSpatialRef();
   coordinate_name_ = spatial_ref->GetAttrValue("geogcs");
+
   // 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;
@@ -128,18 +130,18 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
   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);
+  // 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);
+  // 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)];
-  }
+  //   layer_elevation(x, y) = data[gridMapIndex(0) + width * gridMapIndex(1)];
+  // }
 
   return true;
 }
@@ -169,7 +171,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
@@ -230,7 +232,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