Add VRT support

* Add limits for max map size * And instructions for debugging with gdb * GDAL segfaults in RasterIo() with too large of a dataset unless limits are added * Use vsizip to reduce file size * Add map centering * Needs more transform utilities * Switch to std::array instead of raw double array * Add ROS params for setting map origin * Set RasterIo origin in pixel space based on user supplied geo origin * Switch to GdalDatasetUniquePtr * Remove compile warnings * Add launch files Signed-off-by: Ryan Friedman <[email protected]>
ethz-asl · Mar 24, 2024 · b8c0d1e · b8c0d1e
1 parent 02189d6
commit b8c0d1e
Show file tree

Hide file tree

Showing 11 changed files with 491 additions and 59 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -59,6 +59,14 @@ target_link_libraries(test_tif_loader PUBLIC
   ${PROJECT_NAME}
 )
 
+add_executable(map_publisher
+  src/map_publisher.cpp
+)
+
+target_link_libraries(map_publisher PUBLIC
+  ${PROJECT_NAME}
+)
+
 # Fix for yaml_cpp not resolving correctly on macOS
 # https://github.com/ethz-asl/grid_map_geo/pull/59#discussion_r1474669370
 if (APPLE)
@@ -89,6 +97,7 @@ ament_export_dependencies(Eigen3 GDAL grid_map_core grid_map_msgs grid_map_ros t
 install(
   TARGETS
   test_tif_loader
+  map_publisher
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -97,6 +106,11 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+install(DIRECTORY
+  resources
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 install(DIRECTORY
   rviz
   DESTINATION share/${PROJECT_NAME}/

diff --git a/README.md b/README.md
@@ -14,17 +14,20 @@ 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
-```
+```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
 ```
@@ -67,8 +70,44 @@ 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
+```
+
+**Note:** `grid_map_geo` uses asserts to catch coding errors; they are enabled by default when 
+building in debug mode, and removed from release mode.
+
+## Mixing data from different datums, resolutions, and raster sizes
+
+`grid_map_geo` does not yet support automatically overlaying color data over elevation data from 
+different sources. Currently, color data must be the same datum, resolution and size for both the 
+DEM raster and the color raster.
+
+As an example, `sertig_color.tif` color data can be requested loaded on the SRTM1 DEM at sertig:
+
+```bash
+ros2 launch grid_map_geo load_tif.launch.py params_file:=config/sargans_color_over_srtm1.yaml
+# OR 
+ros2 run --prefix 'gdb -ex run --args' grid_map_geo map_publisher --ros-args --params-file config/sargans_color_over_srtm1.yaml
+```
+
+These datasets are different sizes, different resultions, and use different datums.
+
+Either of these ways of launching cause a floating point crash in `grid_map::getIndexFromLinearIndex`. 
+This limitation may be addressed in a future version.
diff --git a/config/sargans_color_over_srtm1.yaml b/config/sargans_color_over_srtm1.yaml
@@ -0,0 +1,8 @@
+map_publisher:
+    ros__parameters:
+        gdal_dataset_path: /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/N09E047.hgt.zip/N09E047.hgt
+        gdal_dataset_color_path: "resources/sargans_color.tif"
+        max_map_width: 3601
+        max_map_height: 3601
+        map_origin_latitude: 47.033333
+        map_origin_longitude: 9.433333
diff --git a/include/grid_map_geo/grid_map_geo.hpp b/include/grid_map_geo/grid_map_geo.hpp
@@ -34,17 +34,17 @@
 #ifndef GRID_MAP_GEO_H
 #define GRID_MAP_GEO_H
 
-#include <tf2_ros/transform_broadcaster.h>
-
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
+
+// Color map is optional. If left as this default value, color will not be loaded.
+static const std::string COLOR_MAP_DEFAULT_PATH{""};
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <iostream>
 
-#include "transform.hpp"
-struct Location {
-  ESPG espg{ESPG::WGS84};
-  Eigen::Vector3d position{Eigen::Vector3d::Zero()};
-};
+// #include "transform.hpp"
+#include "grid_map_geo/transform.hpp"
 
 class GridMapGeo {
  public:
@@ -98,6 +98,14 @@ class GridMapGeo {
    */
   bool Load(const std::string& map_path) { return 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
    *
@@ -106,16 +114,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)
@@ -184,5 +192,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  // GRID_MAP_GEO_H
diff --git a/include/grid_map_geo/transform.hpp b/include/grid_map_geo/transform.hpp
@@ -35,9 +35,18 @@
 #define GRID_MAP_GEO_TRANSFORM_H
 
 #include <Eigen/Dense>
+#include <array>
+#include <cassert>
 
 enum class ESPG { ECEF = 4978, WGS84 = 4326, WGS84_32N = 32632, CH1903_LV03 = 21781 };
 
+struct Location {
+  ESPG espg{ESPG::WGS84};
+  // <east (lat), north (lng),  up (alt)>
+  //! @todo Switch to geographic_msgs/GeoPoint to make x-y not confusing?
+  Eigen::Vector3d position{Eigen::Vector3d::Zero()};
+};
+
 /**
  * @brief Helper function for transforming using gdal
  *
@@ -58,4 +67,53 @@ Eigen::Vector3d transformCoordinates(ESPG src_coord, ESPG tgt_coord, const Eigen
  */
 Eigen::Vector3d transformCoordinates(ESPG src_coord, const std::string wkt, const Eigen::Vector3d source_coordinates);
 
-#endif  // GRID_MAP_GEO_TRANSFORM_H
+struct Corners {
+  ESPG espg{ESPG::WGS84};
+  Eigen::Vector2d top_left{Eigen::Vector2d::Zero()};
+  Eigen::Vector2d top_right{Eigen::Vector2d::Zero()};
+  Eigen::Vector2d bottom_left{Eigen::Vector2d::Zero()};
+  Eigen::Vector2d bottom_right{Eigen::Vector2d::Zero()};
+};
+
+/**
+ * @brief Helper function converting from image to geo coordinates
+ *
+ * @ref
+ https://gdal.org/tutorials/geotransforms_tut.html#transformation-from-image-coordinate-space-to-georeferenced-coordinate-space
+ * @see GDALApplyGeoTransform
+ *
+ * @param geoTransform The 6-element Geo transform
+ * @param imageCoords The image-coordinates <row, column>, also called <pixel, line>
+
+ * @return The geo-coordinates in <x, y>
+ */
+inline Eigen::Vector2d imageToGeo(const std::array<double, 6> geoTransform, const Eigen::Vector2i imageCoords) {
+  const auto x_pixel = imageCoords.x();
+  const auto y_line = imageCoords.y();
+
+  return {geoTransform.at(0) + x_pixel * geoTransform.at(1) + y_line * geoTransform.at(2),
+          geoTransform.at(3) + x_pixel * geoTransform.at(4) + y_line * geoTransform.at(5)};
+}
+
+/**
+ * @brief Helper function converting from geo to image coordinates. Assumes no rotation.
+ *        Uses the assumption that GT2 and GT4 are zero
+ *
+ * @ref
+ * https://gis.stackexchange.com/questions/384221/calculating-inverse-polynomial-transforms-for-pixel-sampling-when-map-georeferen
+ * @see GDALApplyGeoTransform
+ *
+ * @param geoTransform The 6-element forward Geo transform
+ * @param geoCoords The geo-coordinates in <x, y>
+ *
+ * @return The image-coordinates in <row, column>, also called <pixel, line>
+ */
+inline Eigen::Vector2d geoToImageNoRot(const std::array<double, 6>& geoTransform, const Eigen::Vector2d geoCoords) {
+  assert(geoTransform.at(2) == 0);  // assume no rotation
+  assert(geoTransform.at(4) == 0);  // assume no rotation
+
+  return {(geoCoords.x() - geoTransform.at(0)) / geoTransform.at(1),
+          (geoCoords.y() - geoTransform.at(3)) / geoTransform.at(5)};
+}
+
+#endif
diff --git a/launch/load_map_publisher.launch.py b/launch/load_map_publisher.launch.py
@@ -0,0 +1,18 @@
+<launch>
+    <arg name="rviz" default="true"/>
+    <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="map_publisher" name="map_publisher" namespace="grid_map_geo" output="screen">
+        <param name="gdal_dataset_path" value="$(find-pkg-share grid_map_geo)/resources/sargans.tif"/>
+        <param name="gdal_dataset_color_path" value="$(find-pkg-share grid_map_geo)/resources/sargans_color.tif"/>
+
+        <!-- These values match sargans.tif, but could be increased with no effect on sargans -->
+        <param name="max_map_width" value="748"/>
+        <param name="max_map_height" value="1220"/>
+    </node>
+
+    <group if="$(var rviz)">
+        <node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share grid_map_geo)/rviz/config.rviz"/>
+    </group>
+</launch>
diff --git a/launch/load_map_publisher_launch.xml b/launch/load_map_publisher_launch.xml
@@ -0,0 +1,14 @@
+<launch>
+    <arg name="rviz" default="true"/>
+    <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">
+        <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>
+
+    <group if="$(var rviz)">
+        <node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share grid_map_geo)/rviz/config.rviz"/>
+    </group>
+</launch>
diff --git a/launch/load_tif.launch.py b/launch/load_tif.launch.py
@@ -4,7 +4,7 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
+from launch.conditions import IfCondition, LaunchConfigurationEquals, LaunchConfigurationNotEquals
 from launch.substitutions import LaunchConfiguration
 
 from launch_ros.actions import Node
@@ -28,18 +28,31 @@ def generate_launch_description():
         ],
     )
 
-    # tif loader node
-    tif_loader = Node(
+    # map publisher node
+    map_publisher = Node(
         package="grid_map_geo",
         namespace="grid_map_geo",
-        executable="test_tif_loader",
-        name="tif_loader",
+        executable="map_publisher",
+        name="map_publisher",
         parameters=[
-            {"tif_path": LaunchConfiguration("tif_path")},
-            {"tif_color_path": LaunchConfiguration("tif_color_path")},
+            {"gdal_dataset_path": LaunchConfiguration("gdal_dataset_path")},
+            {"gdal_dataset_color_path": LaunchConfiguration("gdal_dataset_color_path")},
         ],
         output="screen",
         emulate_tty=True,
+        # condition=LaunchConfigurationEquals(LaunchConfiguration("params_file"), "")
+    )
+
+    # map publisher node with params file
+    map_publisher_with_param_file = Node(
+        package="grid_map_geo",
+        namespace="grid_map_geo",
+        executable="map_publisher",
+        name="map_publisher",
+        parameters=[LaunchConfiguration("params_file")],
+        output="screen",
+        emulate_tty=True,
+        condition=LaunchConfigurationNotEquals(LaunchConfiguration("params_file"), "")
     )
 
     # rviz node
@@ -51,8 +64,8 @@ def generate_launch_description():
     )
 
     default_location = "sargans"
-    default_tif_file = "sargans.tif"
-    default_tif_color_file = "sargans_color.tif"
+    default_gdal_dataset = "sargans.tif"
+    default_gdal_color_dataset = "sargans_color.tif"
     return LaunchDescription(
         [
             DeclareLaunchArgument(
@@ -64,17 +77,26 @@ def generate_launch_description():
                 description="Location.",
             ),
             DeclareLaunchArgument(
-                "tif_path",
-                default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_tif_file}',
+                "gdal_dataset_path",
+                default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_gdal_dataset}',
                 description="Full path to the elevation map file.",
             ),
             DeclareLaunchArgument(
-                "tif_color_path",
-                default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_tif_color_file}',
+                "gdal_dataset_color_path",
+                default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_gdal_color_dataset}',
                 description="Full path to the elevation texture file.",
             ),
+            DeclareLaunchArgument(
+                "params_file",
+                default_value="",
+                description="YAML parameter file path.",
+            ),
+
             static_transform_publisher,
-            tif_loader,
+            map_publisher,
+            # map_publisher_with_param_file,
             rviz,
         ]
     )
+
+