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.

Sign up for GitHub

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

Jump to bottom

Support large vrt from ArduPilot SRTM1 terrain server #38

Draft
wants to merge 1 commit into
base: ros2
Choose a base branch
from
Draft

Support large vrt from ArduPilot SRTM1 terrain server #38

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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}
)

Expand All @@ -97,6 +106,11 @@ install(DIRECTORY
DESTINATION share/${PROJECT_NAME}/
)

install(DIRECTORY
resources
DESTINATION share/${PROJECT_NAME}/
)

install(DIRECTORY
rviz
DESTINATION share/${PROJECT_NAME}/
Expand Down
45 changes: 42 additions & 3 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
```
Expand Down Expand Up @@ -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:
Ryanf55 marked this conversation as resolved.
Show resolved Hide resolved

```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.
8 changes: 8 additions & 0 deletions config/sargans_color_over_srtm1.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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'.

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.

map_origin_longitude: 9.433333
35 changes: 24 additions & 11 deletions include/grid_map_geo/grid_map_geo.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
*
Expand All @@ -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)
Expand Down Expand Up @@ -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
60 changes: 59 additions & 1 deletion include/grid_map_geo/transform.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand All @@ -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)};
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.

}

#endif
18 changes: 18 additions & 0 deletions launch/load_map_publisher.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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>
14 changes: 14 additions & 0 deletions launch/load_map_publisher_launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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>
50 changes: 36 additions & 14 deletions launch/load_tif.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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(
Expand All @@ -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,
]
)


Loading