Skip to content

Commit b8c0d1e

Browse files
Ryanf55Ryanf55
committed
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 <ryanfriedman5410+github@gmail.com>
1 parent 02189d6 commit b8c0d1e

11 files changed

+491
-59
lines changed

‎CMakeLists.txt

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,14 @@ target_link_libraries(test_tif_loader PUBLIC
5959
${PROJECT_NAME}
6060
)
6161

62+
add_executable(map_publisher
63+
src/map_publisher.cpp
64+
)
65+
66+
target_link_libraries(map_publisher PUBLIC
67+
${PROJECT_NAME}
68+
)
69+
6270
# Fix for yaml_cpp not resolving correctly on macOS
6371
# https://github.com/ethz-asl/grid_map_geo/pull/59#discussion_r1474669370
6472
if (APPLE)
@@ -89,6 +97,7 @@ ament_export_dependencies(Eigen3 GDAL grid_map_core grid_map_msgs grid_map_ros t
8997
install(
9098
TARGETS
9199
test_tif_loader
100+
map_publisher
92101
DESTINATION lib/${PROJECT_NAME}
93102
)
94103

@@ -97,6 +106,11 @@ install(DIRECTORY
97106
DESTINATION share/${PROJECT_NAME}/
98107
)
99108

109+
install(DIRECTORY
110+
resources
111+
DESTINATION share/${PROJECT_NAME}/
112+
)
113+
100114
install(DIRECTORY
101115
rviz
102116
DESTINATION share/${PROJECT_NAME}/

‎README.md

Lines changed: 42 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -14,17 +14,20 @@ Affiliation: [ETH Zurich, Autonomous Systems Lab](https://asl.ethz.ch/)<br />**
1414

1515
## Setup
1616

17-
Install the dependencies. This package depends on gdal, to read georeferenced images and GeoTIFF files.
17+
Install the dependencies. This package depends on GDAL, to read georeferenced images and DEM files.
1818

1919
Pull in dependencies using rosdep
20-
```
20+
```bash
2121
source /opt/ros/humble/setup.bash
2222
rosdep update
2323
# Assuming the package is cloned in the src folder of a ROS workspace...
2424
rosdep install --from-paths src --ignore-src -y
2525
```
2626

2727
Build the package
28+
29+
```bash
30+
colcon build --mixin release --packages-up-to grid_map_geo
2831
```
2932
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to grid_map_geo
3033
```
@@ -67,8 +70,44 @@ ros2 launch grid_map_geo load_tif_launch.xml
6770
## Running the package
6871

6972
The default launch file can be run as the following command.
70-
```
73+
```bash
7174
source install/setup.bash
7275
ros2 launch grid_map_geo load_tif_launch.xml
7376
```
7477

78+
To debug the map publisher in GDB:
79+
80+
```bash
81+
colcon build --mixin debug --packages-up-to grid_map_geo --symlink-install
82+
source install/setup.bash
83+
84+
# To debug the node under GDB
85+
ros2 run --prefix 'gdb -ex run --args' \
86+
grid_map_geo map_publisher --ros-args \
87+
-p gdal_dataset_path:=install/grid_map_geo/share/grid_map_geo/resources/ap_srtm1.vrt
88+
89+
# To debug from the launch file
90+
ros2 launch grid_map_geo load_vrt_launch.xml
91+
```
92+
93+
**Note:** `grid_map_geo` uses asserts to catch coding errors; they are enabled by default when
94+
building in debug mode, and removed from release mode.
95+
96+
## Mixing data from different datums, resolutions, and raster sizes
97+
98+
`grid_map_geo` does not yet support automatically overlaying color data over elevation data from
99+
different sources. Currently, color data must be the same datum, resolution and size for both the
100+
DEM raster and the color raster.
101+
102+
As an example, `sertig_color.tif` color data can be requested loaded on the SRTM1 DEM at sertig:
103+
104+
```bash
105+
ros2 launch grid_map_geo load_tif.launch.py params_file:=config/sargans_color_over_srtm1.yaml
106+
# OR
107+
ros2 run --prefix 'gdb -ex run --args' grid_map_geo map_publisher --ros-args --params-file config/sargans_color_over_srtm1.yaml
108+
```
109+
110+
These datasets are different sizes, different resultions, and use different datums.
111+
112+
Either of these ways of launching cause a floating point crash in `grid_map::getIndexFromLinearIndex`.
113+
This limitation may be addressed in a future version.

‎config/sargans_color_over_srtm1.yaml

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
map_publisher:
2+
ros__parameters:
3+
gdal_dataset_path: /vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/N09E047.hgt.zip/N09E047.hgt
4+
gdal_dataset_color_path: "resources/sargans_color.tif"
5+
max_map_width: 3601
6+
max_map_height: 3601
7+
map_origin_latitude: 47.033333
8+
map_origin_longitude: 9.433333

‎include/grid_map_geo/grid_map_geo.hpp

Lines changed: 24 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -34,17 +34,17 @@
3434
#ifndef GRID_MAP_GEO_H
3535
#define GRID_MAP_GEO_H
3636

37-
#include <tf2_ros/transform_broadcaster.h>
38-
3937
#include <grid_map_core/GridMap.hpp>
4038
#include <grid_map_core/iterators/GridMapIterator.hpp>
39+
40+
// Color map is optional. If left as this default value, color will not be loaded.
41+
static const std::string COLOR_MAP_DEFAULT_PATH{""};
42+
43+
#include <geometry_msgs/msg/transform_stamped.hpp>
4144
#include <iostream>
4245

43-
#include "transform.hpp"
44-
struct Location {
45-
ESPG espg{ESPG::WGS84};
46-
Eigen::Vector3d position{Eigen::Vector3d::Zero()};
47-
};
46+
// #include "transform.hpp"
47+
#include "grid_map_geo/transform.hpp"
4848

4949
class GridMapGeo {
5050
public:
@@ -98,6 +98,14 @@ class GridMapGeo {
9898
*/
9999
bool Load(const std::string& map_path) { return Load(map_path, ""); }
100100

101+
/**
102+
* @brief Helper function for setting maximum map size. Set to 0 to disable bounds check.
103+
*
104+
* @param pixels_x Maximum number of raster pixels in the X direction
105+
* @param pixels_y Maximum number of raster pixels in the Y direction
106+
*/
107+
void setMaxMapSizePixels(const int pixels_x, const int pixels_y);
108+
101109
/**
102110
* @brief Helper function for loading terrain from path
103111
*
@@ -106,16 +114,16 @@ class GridMapGeo {
106114
* @return true Successfully loaded terrain
107115
* @return false Failed to load terrain
108116
*/
109-
bool Load(const std::string& map_path, const std::string& color_map_path);
117+
bool Load(const std::string& map_path, const std::string color_map_path);
110118

111119
/**
112-
* @brief Initialize grid map from a geotiff file
120+
* @brief Initialize grid map from a GDAL dataset
113121
*
114-
* @param path Path to dsm path (Supported formats are *.tif)
122+
* @param path Path to dsm path (Supported formats are https://gdal.org/drivers/raster/index.html)
115123
* @return true Successfully loaded terrain
116124
* @return false Failed to load terrain
117125
*/
118-
bool initializeFromGeotiff(const std::string& path);
126+
bool initializeFromGdalDataset(const std::string& path);
119127

120128
/**
121129
* @brief Load a color layer from a geotiff file (orthomosaic)
@@ -184,5 +192,10 @@ class GridMapGeo {
184192
Location maporigin_;
185193
std::string frame_id_{""};
186194
std::string coordinate_name_{""};
195+
196+
private:
197+
// Set default map size occupying 4MB RAM assuming 32 bit height precision.
198+
int max_raster_x_size_{1024};
199+
int max_raster_y_size_{1024};
187200
};
188201
#endif // GRID_MAP_GEO_H

‎include/grid_map_geo/transform.hpp

Lines changed: 59 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,9 +35,18 @@
3535
#define GRID_MAP_GEO_TRANSFORM_H
3636

3737
#include <Eigen/Dense>
38+
#include <array>
39+
#include <cassert>
3840

3941
enum class ESPG { ECEF = 4978, WGS84 = 4326, WGS84_32N = 32632, CH1903_LV03 = 21781 };
4042

43+
struct Location {
44+
ESPG espg{ESPG::WGS84};
45+
// <east (lat), north (lng), up (alt)>
46+
//! @todo Switch to geographic_msgs/GeoPoint to make x-y not confusing?
47+
Eigen::Vector3d position{Eigen::Vector3d::Zero()};
48+
};
49+
4150
/**
4251
* @brief Helper function for transforming using gdal
4352
*
@@ -58,4 +67,53 @@ Eigen::Vector3d transformCoordinates(ESPG src_coord, ESPG tgt_coord, const Eigen
5867
*/
5968
Eigen::Vector3d transformCoordinates(ESPG src_coord, const std::string wkt, const Eigen::Vector3d source_coordinates);
6069

61-
#endif // GRID_MAP_GEO_TRANSFORM_H
70+
struct Corners {
71+
ESPG espg{ESPG::WGS84};
72+
Eigen::Vector2d top_left{Eigen::Vector2d::Zero()};
73+
Eigen::Vector2d top_right{Eigen::Vector2d::Zero()};
74+
Eigen::Vector2d bottom_left{Eigen::Vector2d::Zero()};
75+
Eigen::Vector2d bottom_right{Eigen::Vector2d::Zero()};
76+
};
77+
78+
/**
79+
* @brief Helper function converting from image to geo coordinates
80+
*
81+
* @ref
82+
https://gdal.org/tutorials/geotransforms_tut.html#transformation-from-image-coordinate-space-to-georeferenced-coordinate-space
83+
* @see GDALApplyGeoTransform
84+
*
85+
* @param geoTransform The 6-element Geo transform
86+
* @param imageCoords The image-coordinates <row, column>, also called <pixel, line>
87+
88+
* @return The geo-coordinates in <x, y>
89+
*/
90+
inline Eigen::Vector2d imageToGeo(const std::array<double, 6> geoTransform, const Eigen::Vector2i imageCoords) {
91+
const auto x_pixel = imageCoords.x();
92+
const auto y_line = imageCoords.y();
93+
94+
return {geoTransform.at(0) + x_pixel * geoTransform.at(1) + y_line * geoTransform.at(2),
95+
geoTransform.at(3) + x_pixel * geoTransform.at(4) + y_line * geoTransform.at(5)};
96+
}
97+
98+
/**
99+
* @brief Helper function converting from geo to image coordinates. Assumes no rotation.
100+
* Uses the assumption that GT2 and GT4 are zero
101+
*
102+
* @ref
103+
* https://gis.stackexchange.com/questions/384221/calculating-inverse-polynomial-transforms-for-pixel-sampling-when-map-georeferen
104+
* @see GDALApplyGeoTransform
105+
*
106+
* @param geoTransform The 6-element forward Geo transform
107+
* @param geoCoords The geo-coordinates in <x, y>
108+
*
109+
* @return The image-coordinates in <row, column>, also called <pixel, line>
110+
*/
111+
inline Eigen::Vector2d geoToImageNoRot(const std::array<double, 6>& geoTransform, const Eigen::Vector2d geoCoords) {
112+
assert(geoTransform.at(2) == 0); // assume no rotation
113+
assert(geoTransform.at(4) == 0); // assume no rotation
114+
115+
return {(geoCoords.x() - geoTransform.at(0)) / geoTransform.at(1),
116+
(geoCoords.y() - geoTransform.at(3)) / geoTransform.at(5)};
117+
}
118+
119+
#endif

‎launch/load_map_publisher.launch.py

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<launch>
2+
<arg name="rviz" default="true"/>
3+
<arg name="location" default="sargans"/>
4+
<node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
5+
6+
<node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" namespace="grid_map_geo" output="screen">
7+
<param name="gdal_dataset_path" value="$(find-pkg-share grid_map_geo)/resources/sargans.tif"/>
8+
<param name="gdal_dataset_color_path" value="$(find-pkg-share grid_map_geo)/resources/sargans_color.tif"/>
9+
10+
<!-- These values match sargans.tif, but could be increased with no effect on sargans -->
11+
<param name="max_map_width" value="748"/>
12+
<param name="max_map_height" value="1220"/>
13+
</node>
14+
15+
<group if="$(var rviz)">
16+
<node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share grid_map_geo)/rviz/config.rviz"/>
17+
</group>
18+
</launch>

‎launch/load_map_publisher_launch.xml

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
<launch>
2+
<arg name="rviz" default="true"/>
3+
<arg name="location" default="sargans"/>
4+
<node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
5+
6+
<node pkg="grid_map_geo" exec="test_tif_loader" name="test_tif_loader" namespace="grid_map_geo" output="screen">
7+
<param name="tif_path" value="$(find-pkg-share grid_map_geo)/resources/sargans.tif"/>
8+
<param name="tif_color_path" value="$(find-pkg-share grid_map_geo)/resources/sargans_color.tif"/>
9+
</node>
10+
11+
<group if="$(var rviz)">
12+
<node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share grid_map_geo)/rviz/config.rviz"/>
13+
</group>
14+
</launch>

‎launch/load_tif.launch.py

Lines changed: 36 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44

55
from launch import LaunchDescription
66
from launch.actions import DeclareLaunchArgument
7-
from launch.conditions import IfCondition
7+
from launch.conditions import IfCondition, LaunchConfigurationEquals, LaunchConfigurationNotEquals
88
from launch.substitutions import LaunchConfiguration
99

1010
from launch_ros.actions import Node
@@ -28,18 +28,31 @@ def generate_launch_description():
2828
],
2929
)
3030

31-
# tif loader node
32-
tif_loader = Node(
31+
# map publisher node
32+
map_publisher = Node(
3333
package="grid_map_geo",
3434
namespace="grid_map_geo",
35-
executable="test_tif_loader",
36-
name="tif_loader",
35+
executable="map_publisher",
36+
name="map_publisher",
3737
parameters=[
38-
{"tif_path": LaunchConfiguration("tif_path")},
39-
{"tif_color_path": LaunchConfiguration("tif_color_path")},
38+
{"gdal_dataset_path": LaunchConfiguration("gdal_dataset_path")},
39+
{"gdal_dataset_color_path": LaunchConfiguration("gdal_dataset_color_path")},
4040
],
4141
output="screen",
4242
emulate_tty=True,
43+
# condition=LaunchConfigurationEquals(LaunchConfiguration("params_file"), "")
44+
)
45+
46+
# map publisher node with params file
47+
map_publisher_with_param_file = Node(
48+
package="grid_map_geo",
49+
namespace="grid_map_geo",
50+
executable="map_publisher",
51+
name="map_publisher",
52+
parameters=[LaunchConfiguration("params_file")],
53+
output="screen",
54+
emulate_tty=True,
55+
condition=LaunchConfigurationNotEquals(LaunchConfiguration("params_file"), "")
4356
)
4457

4558
# rviz node
@@ -51,8 +64,8 @@ def generate_launch_description():
5164
)
5265

5366
default_location = "sargans"
54-
default_tif_file = "sargans.tif"
55-
default_tif_color_file = "sargans_color.tif"
67+
default_gdal_dataset = "sargans.tif"
68+
default_gdal_color_dataset = "sargans_color.tif"
5669
return LaunchDescription(
5770
[
5871
DeclareLaunchArgument(
@@ -64,17 +77,26 @@ def generate_launch_description():
6477
description="Location.",
6578
),
6679
DeclareLaunchArgument(
67-
"tif_path",
68-
default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_tif_file}',
80+
"gdal_dataset_path",
81+
default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_gdal_dataset}',
6982
description="Full path to the elevation map file.",
7083
),
7184
DeclareLaunchArgument(
72-
"tif_color_path",
73-
default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_tif_color_file}',
85+
"gdal_dataset_color_path",
86+
default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_gdal_color_dataset}',
7487
description="Full path to the elevation texture file.",
7588
),
89+
DeclareLaunchArgument(
90+
"params_file",
91+
default_value="",
92+
description="YAML parameter file path.",
93+
),
94+
7695
static_transform_publisher,
77-
tif_loader,
96+
map_publisher,
97+
# map_publisher_with_param_file,
7898
rviz,
7999
]
80100
)
101+
102+

0 commit comments

Comments
 (0)