Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support large vrt from ArduPilot SRTM1 terrain server #38

Draft
wants to merge 2 commits into
base: ros2
Choose a base branch
from

Conversation

Ryanf55
Copy link
Collaborator

@Ryanf55 Ryanf55 commented Jan 13, 2024

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 Switch from bind to lambda for timer callback #43
  • Use smart pointers for GDAL dataset to fix memory issue Use GDALDatasetUniquePtr over raw pointer #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 Add missing timestamp for grid_map message when publishing #46
  • Move GDAL headers to the implementation (hide them from public API) Hide GDAL includes and link as PRIVATE #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. Validate projection is valid #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 Ryanf55 mentioned this pull request Jan 13, 2024
resources/ap_srtm1.vrt Outdated Show resolved Hide resolved
@Ryanf55 Ryanf55 self-assigned this Jan 14, 2024
@Ryanf55 Ryanf55 changed the title Support large vrt Support large vrt from ArduPilot SRTM1 terrain server Jan 14, 2024
@Ryanf55
Copy link
Collaborator Author

Ryanf55 commented Jan 15, 2024

I attempted to try loading the sargans color tif file at the same time as the SRTM1 dataset for the same area in 9738ba8. 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
Copy link
Member

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

Exactly, you would need #36 to align multiple DEMs

@Ryanf55 Ryanf55 marked this pull request as ready for review January 23, 2024 22:51
Copy link
Member

@Jaeyoung-Lim Jaeyoung-Lim left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.
  • The maximum map size needs to be always defined? Could we only use it when specified?
  • 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
  • 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?

src/map_publisher.cpp Show resolved Hide resolved
gdal_dataset_color_path: "resources/sargans_color.tif"
max_map_width: 3601
max_map_height: 3601
map_origin_latitude: 47.033333
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why would we want to declare this not from the file but externally? Is this maybe because the map alignment was not correct before #36 ?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I needed to declare it somewhere and wanted to provide a reasonable default. Another approach would be to have them as launch arguments. Once we add capability to dynamically load based on UAV location, then this can be removed.

README.md Show resolved Hide resolved
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)};
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am not sure whether we want to be able to freely choose geoCoords within the grid_map_geo framework. At least for me, this additional freedom creates confusion. Wouldn't it be better that we utilize tfs that are clearly defined, so that people doing out of frame stuff can reference the tfs directly?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This function is currently used when a user loads a large VRT, and wants to set the map origin in geo coordinates. Could you give me a little more detail on how one can set the map origin using TF?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The map is always placed at (0, 0, 0) and the tf specifies how that frame_id is placed in the datum (e.g. CH1903). This way the map is aligned automatically through tfs, and we don't need to worry where the rviz is looking at.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If we are dynamically setting the map from the launch file, and eventually loading tiles dynamically, will (0, 0, 0) be where the first map was loaded?
If there's a way to do it in TF, I am interested. The use case I'm trying to support here is user-provided origin anywhere on the SRTM dataset in lat-lon coordinates; the origins of the SRTM tiles are not something the users need to care about because GDAL opens datasets it needs under the hood automatically.

launch/load_vrt_launch.xml Outdated Show resolved Hide resolved
@Ryanf55
Copy link
Collaborator Author

Ryanf55 commented Feb 4, 2024

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
Copy link
Member

Jaeyoung-Lim commented Feb 5, 2024

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.

Eigen::Vector3d map_origin;
map_->getGlobalOrigin(epsg, map_origin);
geometry_msgs::msg::TransformStamped static_transformStamped_;
static_transformStamped_.header.frame_id = map_->getCoordinateName();
static_transformStamped_.child_frame_id = map_->getGridMap().getFrameId();
static_transformStamped_.transform.translation.x = map_origin.x();
static_transformStamped_.transform.translation.y = map_origin.y();
static_transformStamped_.transform.translation.z = 0.0;
static_transformStamped_.transform.rotation.x = 0.0;
static_transformStamped_.transform.rotation.y = 0.0;
static_transformStamped_.transform.rotation.z = 0.0;
static_transformStamped_.transform.rotation.w = 1.0;
tf_broadcaster_->sendTransform(static_transformStamped_);

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?

double mapcenter_n = originY + pixelSizeY * height * 0.5;
maporigin_.espg = ESPG::CH1903_LV03;
maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
if (std::isnan(maporigin_.position.x()) || std::isnan(maporigin_.position.y())) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This would just ignore the actual origin of the map, right? Wouldn't it be better to take whatever is specified in the map?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In the SRTM1 data file, the map origin is latitude=0, longitude=0, and it's in the middle of the ocean and there is no data available. So yes, if the user does not specify a reasonable origin over land, then it will try loading tiles over the ocean which are entirely void data.

For now, I was just hoping you can launch in a non-hard-coded location (it's in the ROS params). In a future PR, I was planning to add more intelligent handling of where to launch the origin.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How does it actually work when we are trying query specific regions of the VRT? Shouldn't we get the origin of the parsed datum when the smaller terrain file is downloaded? The way tif loading works is that you never need to specify where the origin is.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The advantage of using a large VRT is the actual raster tile sizes and origins become data details; it's up to the user to use application-specifics heuristics for setting the origin. For most UAV applications, basing on the current UAV location should work ok.

@Jaeyoung-Lim
Copy link
Member

@Ryanf55 Any updates regarding this direction?

@Ryanf55
Copy link
Collaborator Author

Ryanf55 commented Mar 12, 2024

@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 Ryanf55 force-pushed the support-large-vrt branch 2 times, most recently from 9542a54 to c665858 Compare March 12, 2024 01:13
Copy link
Member

@Jaeyoung-Lim Jaeyoung-Lim left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM! Thanks!

I think we can iterate on how we want to enable the rolling spawning of maps then we will see what the best API would look like

* 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 <ryanfriedman5410+github@gmail.com>
@Ryanf55 Ryanf55 force-pushed the support-large-vrt branch from c665858 to b8c0d1e Compare March 24, 2024 04:24
@Ryanf55
Copy link
Collaborator Author

Ryanf55 commented Mar 24, 2024

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
Copy link
Member

@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

@@ -0,0 +1,8 @@
map_publisher:
ros__parameters:
gdal_dataset_path: /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/N09E047.hgt.zip/N09E047.hgt
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where does the /vsizip/vsicurl come from?

I tried to directly query the terrain server with gdalinfo, but didn't work:

$ gdalinfo https://terrain.ardupilot.org/SRTM1/N09E047.hgt.zip/N09E047.hgt
ERROR 1: An error occurred while creating a virtual connection to the DAP server:Error while reading the URL: https://terrain.ardupilot.org/SRTM1/N09E047.hgt.zip/N09E047.hgt.ver.
The OPeNDAP server returned the following message:
Not Found: The data source or server could not be found.
        Often this means that the OPeNDAP server is missing or needs attention.
        Please contact the server administrator.
gdalinfo failed - unable to open 'https://terrain.ardupilot.org/SRTM1/N09E047.hgt.zip/N09E047.hgt'.

@Jaeyoung-Lim
Copy link
Member

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

rviz_screenshot_2024_04_08-17_19_33

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants