Support large vrt from ArduPilot SRTM1 terrain server

[Ryanf55]

background

This PR adds support for loading terrain from the ArduPilot terrain server. I've manually generated the VRT dataset that refers to all the tiles. It's a massive dataset, and if you try loading it on ros2 branch, it will cause a runtime memory allocation failure crash from trying to read 4TB from the GDAL dataset.

To fix it, I added ROS-configurable limits on the grid map size, and set them to reasonable defaults such that none of the other tif files in your terrain-models/models/*.tif have problems.

Next, in order for the user to set the map origin instead of assuming it's at the center of the dataset, I've added ROS params to set the origin in WGS-84 lat-lon. The vrt launch file runs the ArduPilot vrt dataset with an origin at CMAC, a popular flying field for ArduPilot SITL.

Due to the increased flexibility of loading terrain, additional protections needed to be added. Through some forward and reverse geo transforms, the user-configured origin is validated to be inside of the raster dataset bounds.

To preserve existing behavior, the configured origin defaults to NaN, and falls back to the assumption that the origin will be at the center of the dataset.

other improvements

I've also done some drive-by improvements such as

• Switch from bind to lambda https://github.com/ethz-asl/grid_map_geo/pull/43
• Use smart pointers for GDAL dataset to fix memory issue https://github.com/ethz-asl/grid_map_geo/pull/44
• Replace tif specific naming to gdal_dataset since that's whats more accurately supported
• Improve some error messages
• Add a few assertions to catch errors when run in debug mode
• Add instructions and launch support for running under GDB
• Add const in some places it was missing
• Add ability to just set the path to the DEM file, and then ignore color dataset. Previously, the default was ".", but the validity check was looking for empty string, so no data would be published.
• Timestamp the grid_map message; previously the timestamp was not populated https://github.com/ethz-asl/grid_map_geo/pull/46
• Move GDAL headers to the implementation (hide them from public API) https://github.com/ethz-asl/grid_map_geo/pull/42
• Rename test_tif_loader to map_publisher
• Check Wkt ProjectionRef is valid before continuing to initialize the grid_map. If the projection is unknown, it should not be used. https://github.com/ethz-asl/grid_map_geo/pull/47
• Fix some unused variable compile warnings
• Switch to using std::array for the transforms to increase compile time safety with double arrays

related

This relates to #35, because it allows using a large DEM. It doesn't yet support dynamically moving the grid. This needs to be exposed via params or a ROS service.

follow up

• How to determine espg from the dataset at runtime instead of assuming WGS-84
• whether to use gdal's transform functions
• What's the plan for color data on srtm? Perhaps a good test would be to load the sertig elevation data from SRTM, but then overlay the sertig color file and check it loads?

assumptions

• Ignore reprojection error at large distances
• Ignore home position changes in the autopilot
• split new MapPublisher from original test_tif_load, MapPublisher will "work with any dataset and do everything for you"

[Ryanf55]

I attempted to try loading the sargans color tif file at the same time as the SRTM1 dataset for the same area in 9738ba8c4981b80f96e64e30bbaed3a3706ae5db. The pixel resolutions are different, and the data raster sizes are different. The maps don't align, and it causes the grid_map iterator to crash with a floating point exception. I haven't really been able to dig into it yet, but perhaps this would be OK to merge as-is without the capability to handle layers with different sizes/resolutions/datums? As far as I can tell, this branch does not cause regressions in the original files.

I saw your open PR to align multiple maps may be related...

[Jaeyoung-Lim]

I saw your open PR to align multiple maps may be related...

Exactly, you would need https://github.com/ethz-asl/grid_map_geo/pull/36 to align multiple DEMs

[Ryanf55]

Thanks for the detailed review and ideas!

Sorry that it took me a while to review this.

Thanks for the amazing effort. I think the fact that we can load VRT into grid_map_geo is fascinating!

I have some comments that might be worth discussing.

• I would prefer if we could utilize tf as the primary mechanism of aligning maps.(at least for now) This way we do not need to worry about alignments or storing the origin position of the tif map. It would just automatically align in rviz.

Yea, this sounds like a good idea. Any recommendations?

• The maximum map size needs to be always defined? Could we only use it when specified?

I adjusted the defaults to allow infinite size, but the params for max map size can still be overridden at launch time, which is necessary for the SRTM1 VRT.

• I am a bit worried that the map_publisher will become more and more complicated as we add features. Could we have a simple tif_loader that demonstrates how you can use the API, and maybe a standard map publisher that is actually useful for loading VRT .etc

Yes, I can split it up if you like. There will be some code duplication; hope that's ok.

• I am not sure if I understand the VRT loading, since it seems like it is mounted locally onto your system. (under resources?). What would be the path to creating a demonstration of dynamically loading small map tiles as an entity(vehicle) navigates through terrain? Is there a way to directly communicate with the server?

Fixed, all the examples use the file on the server now.

[Jaeyoung-Lim]

Sorry that it took me a while to review this. Thanks for the amazing effort. I think the fact that we can load VRT into grid_map_geo is fascinating! I have some comments that might be worth discussing. I would prefer if we could utilize tf as the primary mechanism of aligning maps.(at least for now) This way we do not need to worry about alignments or storing the origin position of the tif map. It would just automatically align in rviz.

Yea, this sounds like a good idea. Any recommendations?

In principle, you should not need to do anything, as long as the tf is published with the correct datum: e.g. https://github.com/ethz-asl/grid_map_geo/blob/02189d61cdc30c384e59a76ba1d044e002c39ba9/src/test_tif_loader.cpp#L76-L90

The only problem is that the SRTM DEMs are represented in WGS84, but ROS coordiantes are orthograpthic(e.g. ENU). We can probably ignore the distortion for now and represent everything in WGS 84(e.g. x=46.0 means 46.0 degrees in longitude). Long term we should probably reproject this into UTM to be useful for actual fusion with other map representations.

I adjusted the defaults to allow infinite size, but the params for max map size can still be overridden at launch time, which is necessary for the SRTM1 VRT.

Great!

Yes, I can split it up if you like. There will be some code duplication; hope that's ok.

Sounds good. Since I don't really use the map_publisher in the navigation planners, I think keeping the library simple so that external applications can use it would be the primary use case of this package. (at least for me now)

Fixed, all the examples use the file on the server now.

Awesome, I can try and write a simple application where the map is dynamically spawned around the vehicle position. Although I am not sure what we can do with the map projection problems mentioned in the first point. @Ryanf55 any ideas?

[Jaeyoung-Lim]

@Ryanf55 Any updates regarding this direction?

[Ryanf55]

@Ryanf55 Any updates regarding this direction?

@Ryanf55 Any updates regarding this direction?

Sorry for the delay. Busy time at work and little time for FOSS.

I've done a quick rebase and squash. Let me know what you would like with the launch files, and I'll get those cleaned up. The map publisher and tif loader are now separate.

[Ryanf55]

Remaining tasks I'd like before merge if you aren't in a rush

• [ ] Write tests for all the launch files, disable RVIZ, and make sure the nodes don't crash, and make sure they publish a non-empty map
• [ ] Fix any bugs from testing (I think the original tif loader doesn't work because we need to set the origin to the raster origin.)

Tests I've done so far

map_publisher with ros2 run

ros2 run --prefix 'gdb -ex run --args' grid_map_geo map_publisher --ros-args -p gdal_dataset_path:=/vsizip/vsicurl/https://terrain.ardupilo
t.org/SRTM1/ap_srtm1.zip -p max_map_width:=4096 -p max_map_height:=4096

map_publisher VRT with ros2 launch:

 ros2 launch grid_map_geo load_vrt_launch.xml

TIF loader (single)

 ros2 launch grid_map_geo load_tif.launch.py rviz:=false
ros2 launch grid_map_geo load_tif_launch.xml rviz:=false

TIF loader (multiple):

ros2 launch grid_map_geo load_multiple_tif_launch.xml rviz:=false ==> crash!

 ryan@B650-970:~/Dev/ros2_ws/src/ethz-asl/grid_map_geo/resources$ ros2 launch grid_map_geo load_multiple_tif_launch.xml rviz:=false
[INFO] [launch]: All log files can be found below /home/ryan/.ros/log/2024-04-21-18-35-07-863146-B650-970-677037
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [static_transform_publisher-1]: process started with pid [677038]
[INFO] [test_tif_loader-2]: process started with pid [677040]
[INFO] [test_tif_loader-3]: process started with pid [677042]
[static_transform_publisher-1] [INFO] [1713746107.945762365] [world_map]: Spinning until stopped - publishing transform
[static_transform_publisher-1] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-1] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-1] from 'world' to 'map'
[test_tif_loader-3] [INFO] [1713746107.977345207] [second_tif_loader]: file_path /home/ryan/Dev/ros2_ws/src/ethz-asl/grid_map_geo/install/grid_map_geo/share/grid_map_geo/resources/dischma_valley.tif
[test_tif_loader-3] [INFO] [1713746107.977397025] [second_tif_loader]: color_path /home/ryan/Dev/ros2_ws/src/ethz-asl/grid_map_geo/install/grid_map_geo/share/grid_map_geo/resources/dischma_valley_color.tif
[test_tif_loader-2] [INFO] [1713746107.977656838] [first_tif_loader]: file_path /home/ryan/Dev/ros2_ws/src/ethz-asl/grid_map_geo/install/grid_map_geo/share/grid_map_geo/resources/sargans.tif
[test_tif_loader-2] [INFO] [1713746107.977706372] [first_tif_loader]: color_path /home/ryan/Dev/ros2_ws/src/ethz-asl/grid_map_geo/install/grid_map_geo/share/grid_map_geo/resources/sargans_color.tif
[test_tif_loader-3] 
[test_tif_loader-3] Loading GDAL Dataset for gridmap
[test_tif_loader-2] 
[test_tif_loader-2] Loading GDAL Dataset for gridmap
[test_tif_loader-3] 
[test_tif_loader-3] Wkt ProjectionRef: PROJCS["CH1903 / LV03",GEOGCS["CH1903",DATUM["CH1903",SPHEROID["Bessel 1841",6377397.155,299.1528128,AUTHORITY["EPSG","7004"]],AUTHORITY["EPSG","6149"]],PRIMEM["Greenwich",0,AUTHORITY["EPSG","8901"]],UNIT["degree",0.0174532925199433,AUTHORITY["EPSG","9122"]],AUTHORITY["EPSG","4149"]],PROJECTION["Hotine_Oblique_Mercator_Azimuth_Center"],PARAMETER["latitude_of_center",46.9524055555556],PARAMETER["longitude_of_center",7.43958333333333],PARAMETER["azimuth",90],PARAMETER["rectified_grid_angle",90],PARAMETER["scale_factor",1],PARAMETER["false_easting",600000],PARAMETER["false_northing",200000],UNIT["metre",1,AUTHORITY["EPSG","9001"]],AXIS["Easting",EAST],AXIS["Northing",NORTH],AUTHORITY["EPSG","21781"]]
[test_tif_loader-3] Raster too big. Using a submap of size 1024x1024
[test_tif_loader-3] RasterX: 748 RasterY: 1220 pixelSizeX: 10
[test_tif_loader-3] GMLX: 7480 GMLY: 10240
[test_tif_loader-3] The configured map origin is outside of raster dataset!
[test_tif_loader-2] 
[test_tif_loader-2] Wkt ProjectionRef: PROJCS["CH1903 / LV03",GEOGCS["CH1903",DATUM["CH1903",SPHEROID["Bessel 1841",6377397.155,299.1528128,AUTHORITY["EPSG","7004"]],AUTHORITY["EPSG","6149"]],PRIMEM["Greenwich",0,AUTHORITY["EPSG","8901"]],UNIT["degree",0.0174532925199433,AUTHORITY["EPSG","9122"]],AUTHORITY["EPSG","4149"]],PROJECTION["Hotine_Oblique_Mercator_Azimuth_Center"],PARAMETER["latitude_of_center",46.9524055555556],PARAMETER["longitude_of_center",7.43958333333333],PARAMETER["azimuth",90],PARAMETER["rectified_grid_angle",90],PARAMETER["scale_factor",1],PARAMETER["false_easting",600000],PARAMETER["false_northing",200000],UNIT["metre",1,AUTHORITY["EPSG","9001"]],AXIS["Easting",EAST],AXIS["Northing",NORTH],AUTHORITY["EPSG","21781"]]
[test_tif_loader-2] Raster too big. Using a submap of size 1024x1024
[test_tif_loader-2] RasterX: 748 RasterY: 1220 pixelSizeX: 10
[test_tif_loader-2] GMLX: 7480 GMLY: 10240
[test_tif_loader-3] 
[test_tif_loader-3] Loading color layer from GeoTIFF file for gridmap
[test_tif_loader-3] Width: 748 Height: 1220 Resolution: 10
[test_tif_loader-2] The configured map origin is outside of raster dataset!
[test_tif_loader-2] 
[test_tif_loader-2] Loading color layer from GeoTIFF file for gridmap
[test_tif_loader-2] Width: 748 Height: 1220 Resolution: 10
[ERROR] [test_tif_loader-2]: process has died [pid 677040, exit code -8, cmd '/home/ryan/Dev/ros2_ws/src/ethz-asl/grid_map_geo/install/grid_map_geo/lib/grid_map_geo/test_tif_loader --ros-args -r __node:=first_tif_loader --params-file /tmp/launch_params_ba5wsz53 --params-file /tmp/launch_params_1pi04dlo --params-file /tmp/launch_params_f9irwfok'].
[ERROR] [test_tif_loader-3]: process has died [pid 677042, exit code -8, cmd '/home/ryan/Dev/ros2_ws/src/ethz-asl/grid_map_geo/install/grid_map_geo/lib/grid_map_geo/test_tif_loader --ros-args -r __node:=second_tif_loader --params-file /tmp/launch_params_jz2vaeyt --params-file /tmp/launch_params_qu8tjerw --params-file /tmp/launch_params_mq9akfh4 -r elevation_map:=second_elevation_map'].

[Jaeyoung-Lim]

@Ryanf55 I have started looking into implementing a rolling query depending on vehicle position.

What I learned in between though is that ROS 2 is not really usable in the field over LTE with large messages (elevation maps/images). It seems to be a known issue without a clear solution. Something we need to keep in mind before people use this package in the field

[Jaeyoung-Lim]

Since elevation unit is in m, and the x, y in degrees for WGS84 this is what it looks like when visualized in Rviz: