ethz-asl / grid_map_geo

Geolocalization for grid map using GDAL.

grid_map_geo

This package provides a georeferenced extension to the elevation map grid_map using GDAL, library for raster and vector geospatial data formats

Authors: Jaeyoung Lim
Affiliation: ETH Zurich, Autonomous Systems Lab

Setup

Install the dependencies. This package depends on gdal, to read georeferenced images and GeoTIFF files.

Pull in dependencies using rosdep

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

colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to grid_map_geo

Docker Setup

If your operating system doesn't support ROS 2 humble, docker is a great alternative.

First of all, you have to build the project and create an image like so:

## Assuimg you are in the correct project directory
docker build -t gmg .

To use a shortcut, you may use the following command:

## Assuimg you are in the correct project directory
make docker_build

After the image is created, copy and paste the following command to the terminal to run the image:

## Assuimg you are in the correct project directory
docker run -it --net=host --ipc=host --privileged --env="DISPLAY"  --env="QT_X11_NO_MITSHM=1" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" --volume="${XAUTHORITY}:/root/.Xauthority"  --entrypoint /bin/bash gmg

To use a shortcut, you may use following command:

make docker_run

Running the package at docker image

If you are in the docker image , this project is already sourced and the default launch file can be run as the following command;

ros2 launch grid_map_geo load_tif_launch.xml

Running the package

The default launch file can be run as the following command.

source install/setup.bash
ros2 launch grid_map_geo load_tif_launch.xml

grid_map_geo's Issues

Add CI for ROS2

Problem Description
Currently build tests are missing for the ros2 branch

Error while building grid_map_geo for ROS2 Humble

Hey,
i cant seem to build grid_map_geo for ROS2 Humble

Steps to reproduce:

git clone https://github.com/ethz-asl/grid_map_geo.git --branch ros2 ~/ros2_ws/src/grid_map_geo
cd ~/ros2_ws/
source /opt/ros/humble/setup.bash
rosdep update
rosdep install --from-paths src --ignore-src -y
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False --packages-up-to grid_map_geo

Which then throws following Error:

CMake Error at CMakeLists.txt:24 (add_library):
  Target "grid_map_geo" links to target "grid_map_core::grid_map_core" but
  the target was not found.  Perhaps a find_package() call is missing for an
  IMPORTED target, or an ALIAS target is missing?


CMake Error at src/CMakeLists.txt:7 (add_executable):
  Target "test_tif_loader" links to target "grid_map_core::grid_map_core" but
  the target was not found.  Perhaps a find_package() call is missing for an
  IMPORTED target, or an ALIAS target is missing?


CMake Error at /opt/ros/humble/share/ament_cmake_gtest/cmake/ament_add_gtest_executable.cmake:50 (add_executable):
  Target "grid_map_geo-test" links to target "grid_map_core::grid_map_core"
  but the target was not found.  Perhaps a find_package() call is missing for
  an IMPORTED target, or an ALIAS target is missing?
Call Stack (most recent call first):
  /opt/ros/humble/share/ament_cmake_gtest/cmake/ament_add_gtest_executable.cmake:37 (_ament_add_gtest_executable)
  /opt/ros/humble/share/ament_cmake_gtest/cmake/ament_add_gtest.cmake:68 (ament_add_gtest_executable)
  test/CMakeLists.txt:3 (ament_add_gtest)

Python Launch: Switch from os to pathlib

Purpose

The pathlib library is more pythonic than calling os.path for managings paths.

Currently, launch/load_tif.launch.py uses the os module. let's switch to pathlib.

How to run it

colcon build --packages-up-to grid_map_geo
source install/setup.bash
ros2 launch grid_map_geo load_tif.launch.py

Required tests

• Switch to pathlib
• Share terminal output of successful launch
• Add the above steps to the README because it's missing

DOCS: gh pages permissions errors on publish

The github pages for doxygen API has permissions errors, despite following the docs from the workflow.

This is blocking other PR's, so if I can't find a fix, I'll just remove that job till a later date.
The more important thing is have a working doxyfile, and later document the public header fully (fix all the current doxygen errors).

[meta] Release grid_map_geo binaries with bloom

Purpose

This issue is to keep track of the tasks needed to release this repository through bloom. Once the library has a stable-ish API, it makes sense to release as binaries instead of requiring consumers, like terrain-navigation, to compile this from source.

Pre-requisites

• ros/rosdistro#40449
• #35 including both ArduPilot VRT support and dynamic load/unload
• Separate out the ROS layer from the core grid_map_geo library, perhaps by dividing it into two packages
• Add an example of a plane flying in SITL and demonstrate query of map data along the flight and visualize in rviz
• Finalize how users should specify map size limits
• Verify that poor configuration will not cause a runtime crash of the library (add some test cases that can be run in CI)
• Add CI step to check the export of the library is correct and not missing dependencies #59
• Create a release team

Final checks

• Verify the package.xml has a version starting with 0, which lets consumers know this is experimental and not necessarily ABI stable yet (We can release version 1.0 once terrain navigation has been used by enough users)
• Generate a changelog
• Follow this tutorial to release the package

test_grid_map_geo.launch is not valid in the repo

Hi,

I wanted to test the project so I setup the project as you describe in the repository but "test_grid_map_geo.launch" is not valid in the repo for the demo.

thanks.

Dynamically load terrain files at runtime

Purpose

This issue is to capture the desire and design, for dynamically loading terrain.

Current Behavior

Currently, grid_map_geo supports loading a single .tif file. Before flight, a user must create the .tif file for the region they plan to fly, and then modify their launch or parameter to point to the correct file. While this works great for small tests where the user is familiar with terrain generation and flies areas known well in advance, it doesn't work for casual users who just want terrain data working wherever they fly automatically.

Furthermore, it only works with tif files, so users can't use any terrain data supplied by other tools.

Desired Behavior

A user can fetch terrain using a common terrain server, such as ArduPilot Terrain Generator. A processing step occurs onboard, and then the terrain tile(s) are suitable for use in grid_map_geo. grid_map_geo should be able to work with more than just .tif files. If the user requests color data loaded, the color data may be in a different datum/resolution/raster size than the DEM data.

Proposed Design

Utilize GDAL's VRT format as the pre-processing step for terrain. This would allow loading multiple terrain tiles, either to cover more area, or to allow overlapping tiles of differing resolution. While the VRT generation could be done manually with a call to gdalbuildvrt, it could be automated in the future, or generated server-side.

Next, modify the grid_map_geo implementation to work off either a single .tif file, or also given a path to a .vrt file.

Finally, allow grid_map_geo to support loading partial terrain, instead of the entire vrt dataset, and then allow dynamic unload/load of regions through additional API calls.

Using GDAL tools with the ArduPilot terrain database

Fetching terrain

One can fetch terrain from the ArduPilot terrain server like so:

$ gdalinfo /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/N00E006.hgt.zip/N00E006.hgt
Driver: SRTMHGT/SRTMHGT File Format
Files: /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/N00E006.hgt.zip/N00E006.hgt
Size is 3601, 3601
Coordinate System is:
GEOGCRS["WGS 84",
    DATUM["World Geodetic System 1984",
        ELLIPSOID["WGS 84",6378137,298.257223563,
            LENGTHUNIT["metre",1]]],
    PRIMEM["Greenwich",0,
        ANGLEUNIT["degree",0.0174532925199433]],
    CS[ellipsoidal,2],
        AXIS["geodetic latitude (Lat)",north,
            ORDER[1],
            ANGLEUNIT["degree",0.0174532925199433]],
        AXIS["geodetic longitude (Lon)",east,
            ORDER[2],
            ANGLEUNIT["degree",0.0174532925199433]],
    ID["EPSG",4326]]
Data axis to CRS axis mapping: 2,1
Origin = (5.999861111111112,1.000138888888889)
Pixel Size = (0.000277777777778,-0.000277777777778)
Metadata:
  AREA_OR_POINT=Point
Corner Coordinates:
Upper Left  (   5.9998611,   1.0001389) (  5d59'59.50"E,  1d 0' 0.50"N)
Lower Left  (   5.9998611,  -0.0001389) (  5d59'59.50"E,  0d 0' 0.50"S)
Upper Right (   7.0001389,   1.0001389) (  7d 0' 0.50"E,  1d 0' 0.50"N)
Lower Right (   7.0001389,  -0.0001389) (  7d 0' 0.50"E,  0d 0' 0.50"S)
Center      (   6.5000000,   0.5000000) (  6d30' 0.00"E,  0d30' 0.00"N)
Band 1 Block=3601x1 Type=Int16, ColorInterp=Undefined
  NoData Value=-32768
  Unit Type: m

Building a VRT

For a single file:

gdalbuildvrt index.vrt /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/N00E006.hgt.zip/N00E006.hgt

<VRTDataset rasterXSize="3601" rasterYSize="3601">
  <SRS dataAxisToSRSAxisMapping="2,1">GEOGCS["WGS 84",DATUM["WGS_1984",SPHEROID["WGS 84",6378137,298.257223563,AUTHORITY["EPSG","7030"]],AUTHORITY["EPSG","6326"]],PRIMEM["Greenwich",0,AUTHORITY["EPSG","8901"]],UNIT["degree",0.0174532925199433,AUTHORITY["EPSG","9122"]],AXIS["Latitude",NORTH],AXIS["Longitude",EAST],AUTHORITY["EPSG","4326"]]</SRS>
  <GeoTransform>  5.9998611111111115e+00,  2.7777777777777778e-04,  0.0000000000000000e+00,  1.0001388888888889e+00,  0.0000000000000000e+00, -2.7777777777777778e-04</GeoTransform>
  <VRTRasterBand dataType="Int16" band="1">
    <NoDataValue>-32768</NoDataValue>
    <ComplexSource>
      <SourceFilename relativeToVRT="0">/vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/N00E006.hgt.zip/N00E006.hgt</SourceFilename>
      <SourceBand>1</SourceBand>
      <SourceProperties RasterXSize="3601" RasterYSize="3601" DataType="Int16" BlockXSize="3601" BlockYSize="1" />
      <SrcRect xOff="0" yOff="0" xSize="3601" ySize="3601" />
      <DstRect xOff="0" yOff="0" xSize="3601" ySize="3601" />
      <NODATA>-32768</NODATA>
    </ComplexSource>
  </VRTRasterBand>
</VRTDataset>

Or, for multiple:

gdalbuildvrt index.vrt /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM3/North_America/N10W110.hgt.zip/N10W110.hgt /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM3/North_America/N15W062.hgt.zip/N15W062.hgt

<VRTDataset rasterXSize="58801" rasterYSize="7201">
  <SRS dataAxisToSRSAxisMapping="2,1">GEOGCS["WGS 84",DATUM["WGS_1984",SPHEROID["WGS 84",6378137,298.257223563,AUTHORITY["EPSG","7030"]],AUTHORITY["EPSG","6326"]],PRIMEM["Greenwich",0,AUTHORITY["EPSG","8901"]],UNIT["degree",0.0174532925199433,AUTHORITY["EPSG","9122"]],AXIS["Latitude",NORTH],AXIS["Longitude",EAST],AUTHORITY["EPSG","4326"]]</SRS>
  <GeoTransform> -1.1000041666666667e+02,  8.3333333333333339e-04,  0.0000000000000000e+00,  1.6000416666666666e+01,  0.0000000000000000e+00, -8.3333333333333339e-04</GeoTransform>
  <VRTRasterBand dataType="Int16" band="1">
    <NoDataValue>-32768</NoDataValue>
    <ComplexSource>
      <SourceFilename relativeToVRT="0">/vsizip/vsicurl/https://terrain.ardupilot.org/SRTM3/North_America/N10W110.hgt.zip/N10W110.hgt</SourceFilename>
      <SourceBand>1</SourceBand>
      <SourceProperties RasterXSize="1201" RasterYSize="1201" DataType="Int16" BlockXSize="1201" BlockYSize="1" />
      <SrcRect xOff="0" yOff="0" xSize="1201" ySize="1201" />
      <DstRect xOff="0" yOff="6000" xSize="1201" ySize="1201" />
      <NODATA>-32768</NODATA>
    </ComplexSource>
    <ComplexSource>
      <SourceFilename relativeToVRT="0">/vsizip/vsicurl/https://terrain.ardupilot.org/SRTM3/North_America/N15W062.hgt.zip/N15W062.hgt</SourceFilename>
      <SourceBand>1</SourceBand>
      <SourceProperties RasterXSize="1201" RasterYSize="1201" DataType="Int16" BlockXSize="1201" BlockYSize="1" />
      <SrcRect xOff="0" yOff="0" xSize="1201" ySize="1201" />
      <DstRect xOff="57600" yOff="0" xSize="1201" ySize="1201" />
      <NODATA>-32768</NODATA>
    </ComplexSource>
  </VRTRasterBand>
</VRTDataset>

Query height of a single point at CMAC, which returns 467m elevation

$ gdallocationinfo /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/S35E149.hgt.zip/S35E149.hgt  149.159350 35.363272  
Report:
  Location: (149P,35L)
  Band 1:
    Value: 467

Further References

• MavLink Terrain Protocol
• GCS push terrain data for offline use
• ArduPilot .dat file format and tools
• https://gist.github.com/scottyhq/20d36c9217298e5dfe284f6a2f25dea8

Make test_tif_loader publish quicker by taking advantage of transient local QOS

Purpose

The test_tif_loader has a 5 second timer to publish the grid map. The way timers work in ROS is that they aren't executed when constructed; they must go through the first period of time.

The effect is that there is always a 5 second wait before the map shows up in RVIZ.

Goal for this ticket

Make the map publish earlier, as soon as it's ready to be published. Regardless of when RVIZ is started, RVIZ should show the map as soon as it subscribes to the map topic.

Hints

What if you call timer_callback(); at the end of the MapPublisher constructor - seems like it publishes before RVIZ is up and ready to receive the message.
What if you change the quality of service of the publisher to reliable, history length 1, transient local durability to support the following behavior:

To achieve a “latched” topic that is visible to late subscribers, both the publisher and subscriber must agree to use ‘Transient Local’.

Finally, if the QoS is changed on the publisher, rviz should be updated to do the same.

Longer term

grid_map_geo should only publish the map when it changes, rather than on a timer, and take advantage of ROS QoS. Is there a reason we aren't using transient local QoS for the map?

Reference

• https://docs.ros.org/en/humble/Concepts/Intermediate/About-Quality-of-Service-Settings.html#qos-profiles

no data in eleveation map

Hi,
Thanks for the nice package. However the alligning doesnt seem to work in my case

SUMMARY
========

PARAMETERS
 * /rosdistro: noetic
 * /rosversion: 1.15.14
 * /test_tif_loader/tif_path: /home/ahmad/works...

NODES
  /
    rviz (rviz/rviz)
    test_tif_loader (grid_map_geo/test_tif_loader)
    world_map (tf/static_transform_publisher)

auto-starting new master
process[master]: started with pid [236172]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 9280e2fe-1700-11ed-b6a4-7b9e3271b9fe
process[rosout-1]: started with pid [236213]
started core service [/rosout]
process[world_map-2]: started with pid [236216]
process[test_tif_loader-3]: started with pid [236217]
process[rviz-4]: started with pid [236222]

Loading GeoTIFF file for gridmap

Wkt ProjectionRef: GEOGCS["WGS 84",DATUM["WGS_1984",SPHEROID["WGS 84",6378137,298.257223563,AUTHORITY["EPSG","7030"]],TOWGS84[0,0,0,0,0,0,0],AUTHORITY["EPSG","6326"]],PRIMEM["Greenwich",0],UNIT["degree",0.0174532925199433,AUTHORITY["EPSG","9122"]],AXIS["Latitude",NORTH],AXIS["Longitude",EAST],AUTHORITY["EPSG","4326"]]
Width: 3139 Height: 3361 Resolution: 1.43701e-06
[GridMapGeo] Not aligning terrain!

this refers to

grid_map_geo/src/grid_map_geo.cpp

Line 112 in 4a14be7

std::cout << "[GridMapGeo] Not aligning terrain!" << std::endl;

but i dont understand what you are doing in the lines above that..

Port to ROS 2

I'd like to use this in ROS 2 to support the SmartRTL feature in ArduPilot. Please assign me this issue, and I'll issue a PR for the port to ROS 2 as soon as it's ready.

Since grid_map already has ROS 2 support, nothing is blocking this work.

In scope:

• Replace catkin with colcon
• Update roscpp with rclcpp
• Update README for ROS 2 usage
• Modernize CMake for CMake 3.12 features
• Remove compile warnings/errors that show up in GCC11 (Ubuntu 22)
• Works on Humble

Later:

• Releasing binaries to the ROS build farm with bloom
• CI

No resources

Hi @Jaeyoung-Lim
I am working on ros2 branch but error shows there are missing resources as below:

• resources/sargans.tif
• resources/sargans_color.tif

Let me know how can I get those files.
Thanks

Media Place holders