diff --git a/CMakeLists.txt b/CMakeLists.txt
index 78fecd0..f8f2dc8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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)
@@ -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}
)
@@ -97,6 +106,11 @@ install(DIRECTORY
DESTINATION share/${PROJECT_NAME}/
)
+install(DIRECTORY
+ resources
+ DESTINATION share/${PROJECT_NAME}/
+)
+
install(DIRECTORY
rviz
DESTINATION share/${PROJECT_NAME}/
diff --git a/README.md b/README.md
index da2b88a..8ded061 100644
--- a/README.md
+++ b/README.md
@@ -14,10 +14,10 @@ Affiliation: [ETH Zurich, Autonomous Systems Lab](https://asl.ethz.ch/)
**
## 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...
@@ -25,6 +25,9 @@ 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
```
@@ -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:
+
+```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.
diff --git a/config/sargans_color_over_srtm1.yaml b/config/sargans_color_over_srtm1.yaml
new file mode 100644
index 0000000..89f4739
--- /dev/null
+++ b/config/sargans_color_over_srtm1.yaml
@@ -0,0 +1,8 @@
+map_publisher:
+ ros__parameters:
+ gdal_dataset_path: /vsizip/vsicurl/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
+ map_origin_longitude: 9.433333
diff --git a/include/grid_map_geo/grid_map_geo.hpp b/include/grid_map_geo/grid_map_geo.hpp
index d71c8e2..e0fe47c 100644
--- a/include/grid_map_geo/grid_map_geo.hpp
+++ b/include/grid_map_geo/grid_map_geo.hpp
@@ -34,17 +34,17 @@
#ifndef GRID_MAP_GEO_H
#define GRID_MAP_GEO_H
-#include
-
#include
#include
+
+// Color map is optional. If left as this default value, color will not be loaded.
+static const std::string COLOR_MAP_DEFAULT_PATH{""};
+
+#include
#include
-#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:
@@ -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
*
@@ -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)
@@ -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
diff --git a/include/grid_map_geo/transform.hpp b/include/grid_map_geo/transform.hpp
index 1518d6a..14d6603 100644
--- a/include/grid_map_geo/transform.hpp
+++ b/include/grid_map_geo/transform.hpp
@@ -35,9 +35,18 @@
#define GRID_MAP_GEO_TRANSFORM_H
#include
+#include
+#include
enum class ESPG { ECEF = 4978, WGS84 = 4326, WGS84_32N = 32632, CH1903_LV03 = 21781 };
+struct Location {
+ ESPG espg{ESPG::WGS84};
+ //
+ //! @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
*
@@ -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 , also called
+
+ * @return The geo-coordinates in
+ */
+inline Eigen::Vector2d imageToGeo(const std::array 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
+ *
+ * @return The image-coordinates in , also called
+ */
+inline Eigen::Vector2d geoToImageNoRot(const std::array& 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)};
+}
+
+#endif
diff --git a/launch/load_map_publisher.launch.py b/launch/load_map_publisher.launch.py
new file mode 100644
index 0000000..f12ceb6
--- /dev/null
+++ b/launch/load_map_publisher.launch.py
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/load_map_publisher_launch.xml b/launch/load_map_publisher_launch.xml
new file mode 100644
index 0000000..eace660
--- /dev/null
+++ b/launch/load_map_publisher_launch.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/load_tif.launch.py b/launch/load_tif.launch.py
index 0692a54..63c5edb 100644
--- a/launch/load_tif.launch.py
+++ b/launch/load_tif.launch.py
@@ -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
@@ -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
@@ -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(
@@ -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,
]
)
+
+
diff --git a/launch/load_vrt_launch.xml b/launch/load_vrt_launch.xml
new file mode 100644
index 0000000..efafa4c
--- /dev/null
+++ b/launch/load_vrt_launch.xml
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp
index 45ecd06..1f0914d 100644
--- a/src/grid_map_geo.cpp
+++ b/src/grid_map_geo.cpp
@@ -40,6 +40,7 @@
#include "grid_map_geo/grid_map_geo.hpp"
#include
+#include
#include
#include
#include
@@ -58,33 +59,67 @@
#include
#endif
+/**
+ * @brief Helper function for getting dataset corners
+ * Inspired by gdalinfo_lib.cpp::GDALInfoReportCorner()
+ *
+ * @param datasetPtr The pointer to the dataset
+ * @param corners The returned corners in the geographic coordinates
+ * @return
+ */
+inline bool getGeoCorners(const GDALDatasetUniquePtr &datasetPtr, Corners &corners) {
+ std::array geoTransform;
+
+ // https://gdal.org/tutorials/geotransforms_tut.html#introduction-to-geotransforms
+ if (!datasetPtr->GetGeoTransform(geoTransform.data()) == CE_None) {
+ return false;
+ }
+
+ const auto raster_width = datasetPtr->GetRasterXSize();
+ const auto raster_height = datasetPtr->GetRasterYSize();
+
+ corners.top_left = imageToGeo(geoTransform, {0, 0});
+ corners.top_right = imageToGeo(geoTransform, {raster_width, 0});
+ corners.bottom_left = imageToGeo(geoTransform, {0, raster_height});
+ corners.bottom_right = imageToGeo(geoTransform, {raster_width, raster_height});
+
+ return true;
+}
+
GridMapGeo::GridMapGeo(const std::string &frame_id) { frame_id_ = frame_id; }
GridMapGeo::~GridMapGeo() {}
-bool GridMapGeo::Load(const std::string &map_path, const std::string &color_map_path) {
- bool loaded = initializeFromGeotiff(map_path);
- if (!color_map_path.empty()) { // Load color layer if the color path is nonempty
+void GridMapGeo::setMaxMapSizePixels(const int pixels_x, const int pixels_y) {
+ // Use int type to match GDAL , but validate it's a positive value.
+ assert(pixels_x >= 0);
+ assert(pixels_y >= 0);
+
+ max_raster_x_size_ = pixels_x;
+ max_raster_y_size_ = pixels_y;
+}
+
+bool GridMapGeo::Load(const std::string &map_path, const std::string color_map_path) {
+ bool loaded = initializeFromGdalDataset(map_path);
+ if (color_map_path != COLOR_MAP_DEFAULT_PATH) { // Load color layer if the color path is nonempty
bool color_loaded = addColorFromGeotiff(color_map_path);
}
if (!loaded) return false;
return true;
}
-bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
+bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
GDALAllRegister();
const auto dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(GDALOpen(path.c_str(), GA_ReadOnly)));
if (!dataset) {
- std::cout << "Failed to open" << std::endl;
+ std::cout << __func__ << "Failed to open '" << path << "'" << std::endl;
return false;
}
- std::cout << std::endl << "Loading GeoTIFF file for gridmap" << std::endl;
+ std::cout << std::endl << "Loading GDAL Dataset for gridmap" << std::endl;
- double originX, originY, pixelSizeX, pixelSizeY;
+ double pixelSizeX, pixelSizeY;
std::array geoTransform;
if (dataset->GetGeoTransform(geoTransform.data()) == CE_None) {
- originX = geoTransform[0];
- originY = geoTransform[3];
pixelSizeX = geoTransform[1];
pixelSizeY = geoTransform[5];
} else {
@@ -104,41 +139,82 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
const OGRSpatialReference *spatial_ref = dataset->GetSpatialRef();
coordinate_name_ = spatial_ref->GetAttrValue("geogcs");
+
// Get image metadata
- unsigned width = dataset->GetRasterXSize();
- unsigned height = dataset->GetRasterYSize();
+ const auto raster_width = dataset->GetRasterXSize();
+ const auto raster_height = dataset->GetRasterYSize();
+
+ if (raster_width > max_raster_x_size_ || raster_height > max_raster_y_size_) {
+ std::cout << "Raster too big. Using a submap of size " << max_raster_x_size_ << "x" << max_raster_y_size_
+ << std::endl;
+ }
double resolution = pixelSizeX;
- std::cout << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
+ std::cout << "RasterX: " << raster_width << " RasterY: " << raster_height << " pixelSizeX: " << pixelSizeX
+ << std::endl;
+
+ // Limit grid map size to not exceed memory limitations
+ const auto grid_width = std::min(raster_width, max_raster_x_size_);
+ const auto grid_height = std::min(raster_height, max_raster_y_size_);
// pixelSizeY is negative because the origin of the image is the north-east corner and positive
// Y pixel coordinates go towards the south
- const double lengthX = resolution * width;
- const double lengthY = resolution * height;
+ const double lengthX = resolution * grid_width;
+ const double lengthY = resolution * grid_height;
grid_map::Length length(lengthX, lengthY);
+ std::cout << "GMLX: " << lengthX << " GMLY: " << lengthY << std::endl;
+
+ //! @todo use WGS-84 as map origin if specified
+ //! @todo
+ //! @todo check the origin point is non-void (not in the middle of the ocean)
+ Corners corners;
+ if (!getGeoCorners(dataset, corners)) {
+ std::cerr << "Failed to get geographic corners of dataset!" << std::endl;
+ return false;
+ }
- double mapcenter_e = originX + pixelSizeX * width * 0.5;
- 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())) {
+ //! @todo Figure out how to not hard code the espg, perhaps using the dataset GEOGCRS attribute.
+ // maporigin_.espg = ESPG::CH1903_LV03;
+ const double mapcenter_e = (corners.top_left.x() + corners.bottom_right.x()) / 2.0;
+ const double mapcenter_n = (corners.top_left.y() + corners.bottom_right.y()) / 2.0;
+ maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
+ } else {
+ if (maporigin_.position.x() < corners.top_left.x() || maporigin_.position.x() > corners.bottom_right.x() ||
+ maporigin_.position.y() < corners.bottom_right.y() || maporigin_.position.y() > corners.top_left.y()) {
+ std::cerr << "The configured map origin is outside of raster dataset!" << std::endl;
+ return false;
+ }
+ }
Eigen::Vector2d position{Eigen::Vector2d::Zero()};
grid_map_.setGeometry(length, resolution, position);
grid_map_.setFrameId(frame_id_);
grid_map_.add("elevation");
+
+ const auto raster_count = dataset->GetRasterCount();
+ assert(raster_count == 1); // expect only elevation data, otherwise it's the wrong dataset.
GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
- std::vector data(width * height, 0.0f);
- elevationBand->RasterIO(GF_Read, 0, 0, width, height, &data[0], width, height, GDT_Float32, 0, 0);
+ // Compute the center in pixel space, then get the raster bounds nXOff and nYOff to extract with RasterIO
+ const auto center_px = geoToImageNoRot(geoTransform, {maporigin_.position.x(), maporigin_.position.y()});
+ const auto raster_io_x_offset = center_px.x() - grid_width / 2;
+ const auto raster_io_y_offset = center_px.y() - grid_height / 2;
+ std::vector data(grid_width * grid_height, 0.0f);
+ const auto raster_err = elevationBand->RasterIO(GF_Read, raster_io_x_offset, raster_io_y_offset, grid_width,
+ grid_height, &data[0], grid_width, grid_height, GDT_Float32, 0, 0);
+ if (raster_err != CPLE_None) {
+ return false;
+ }
grid_map::Matrix &layer_elevation = grid_map_["elevation"];
for (grid_map::GridMapIterator iterator(grid_map_); !iterator.isPastEnd(); ++iterator) {
const grid_map::Index gridMapIndex = *iterator;
// TODO: This may be wrong if the pixelSizeY > 0
- int x = width - 1 - gridMapIndex(0);
+ int x = grid_width - 1 - gridMapIndex(0);
int y = gridMapIndex(1);
- layer_elevation(x, y) = data[gridMapIndex(0) + width * gridMapIndex(1)];
+ layer_elevation(x, y) = data[gridMapIndex(0) + grid_width * gridMapIndex(1)];
}
return true;
@@ -148,16 +224,14 @@ bool GridMapGeo::addColorFromGeotiff(const std::string &path) {
GDALAllRegister();
const auto dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(GDALOpen(path.c_str(), GA_ReadOnly)));
if (!dataset) {
- std::cout << "Failed to open" << std::endl;
+ std::cout << __func__ << "Failed to open '" << path << "'" << std::endl;
return false;
}
std::cout << std::endl << "Loading color layer from GeoTIFF file for gridmap" << std::endl;
- double originX, originY, pixelSizeX, pixelSizeY;
+ double pixelSizeX, pixelSizeY;
std::array geoTransform;
if (dataset->GetGeoTransform(geoTransform.data()) == CE_None) {
- originX = geoTransform[0];
- originY = geoTransform[3];
pixelSizeX = geoTransform[1];
pixelSizeY = geoTransform[5];
} else {
@@ -209,16 +283,14 @@ bool GridMapGeo::addLayerFromGeotiff(const std::string &layer_name, const std::s
GDALAllRegister();
const auto dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(GDALOpen(path.c_str(), GA_ReadOnly)));
if (!dataset) {
- std::cout << "Failed to open" << std::endl;
+ std::cout << __func__ << "Failed to open '" << path << "'" << std::endl;
return false;
}
std::cout << std::endl << "Loading color layer from GeoTIFF file for gridmap" << std::endl;
- double originX, originY, pixelSizeX, pixelSizeY;
+ double pixelSizeX, pixelSizeY;
std::array geoTransform;
if (dataset->GetGeoTransform(geoTransform.data()) == CE_None) {
- originX = geoTransform[0];
- originY = geoTransform[3];
pixelSizeX = geoTransform[1];
pixelSizeY = geoTransform[5];
} else {
diff --git a/src/map_publisher.cpp b/src/map_publisher.cpp
new file mode 100644
index 0000000..8c001d2
--- /dev/null
+++ b/src/map_publisher.cpp
@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2022 Jaeyoung Lim. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @brief Node to test planner in the view utility map
+ *
+ *
+ * @author Jaeyoung Lim
+ */
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "grid_map_geo/grid_map_geo.hpp"
+
+using namespace std::chrono_literals;
+
+class MapPublisher : public rclcpp::Node {
+ public:
+ MapPublisher() : Node("map_publisher") {
+ original_map_pub_ = this->create_publisher("elevation_map", 1);
+ const std::string frame_id = this->declare_parameter("frame_id", "map");
+ const std::string gdal_dataset_path = this->declare_parameter("gdal_dataset_path", ".");
+ const std::string color_path = this->declare_parameter("gdal_dataset_color_path", COLOR_MAP_DEFAULT_PATH);
+ rcl_interfaces::msg::ParameterDescriptor max_map_descriptor;
+ max_map_descriptor.read_only = true;
+ max_map_descriptor.description =
+ "Maximum number of raster pixels able to be loaded. \
+ Useful when working with large raster datasets to limit memory usage. \
+ Set to 0 to disable limits (may cause runtime crash).";
+ rcl_interfaces::msg::IntegerRange max_map_descriptor_int_range;
+
+ max_map_descriptor_int_range.from_value = 0;
+ max_map_descriptor_int_range.to_value = std::numeric_limits::max();
+ max_map_descriptor.integer_range.push_back(max_map_descriptor_int_range);
+
+ const uint16_t max_map_width =
+ std::clamp(this->declare_parameter("max_map_width", std::numeric_limits::max(), max_map_descriptor),
+ max_map_descriptor_int_range.from_value, max_map_descriptor_int_range.to_value);
+ const uint16_t max_map_height =
+ std::clamp(this->declare_parameter("max_map_height", std::numeric_limits::max(), max_map_descriptor),
+ max_map_descriptor_int_range.from_value, max_map_descriptor_int_range.to_value);
+
+ RCLCPP_INFO_STREAM(get_logger(), "gdal_dataset_path: '" << gdal_dataset_path << "'");
+ RCLCPP_INFO_STREAM(get_logger(), "gdal_dataset_color_path: '" << color_path << "'");
+ tf_broadcaster_ = std::make_shared(this);
+
+ map_ = std::make_shared(frame_id);
+ map_->setMaxMapSizePixels(max_map_width, max_map_height);
+
+ rcl_interfaces::msg::ParameterDescriptor origin_descriptor;
+ origin_descriptor.read_only = true;
+ origin_descriptor.description = "Map origin latitude (WGS-84) in degrees.";
+ rcl_interfaces::msg::FloatingPointRange origin_range;
+
+ origin_range.from_value = -90.0;
+ origin_range.to_value = 90.0;
+ origin_descriptor.floating_point_range.push_back(origin_range);
+
+ static_assert(std::numeric_limits::has_quiet_NaN == true, "Need quiet NaN for default value");
+ const auto map_origin_latitude = std::clamp(
+ this->declare_parameter("map_origin_latitude", std::numeric_limits::quiet_NaN(), origin_descriptor),
+ origin_range.from_value, origin_range.to_value);
+
+ origin_range.from_value = -180.0;
+ origin_range.to_value = 180.0;
+ origin_descriptor.floating_point_range.at(0) = origin_range;
+
+ origin_descriptor.description = "Map origin longitude (WGS-84) in degrees.";
+ const auto map_origin_longitude = std::clamp(
+ this->declare_parameter("map_origin_longitude", std::numeric_limits::quiet_NaN(), origin_descriptor),
+ origin_range.from_value, origin_range.to_value);
+
+ map_ = std::make_shared();
+ map_->setMaxMapSizePixels(max_map_width, max_map_height);
+ map_->setGlobalOrigin(ESPG::WGS84, Eigen::Vector3d(map_origin_longitude, map_origin_latitude, 0.0));
+ map_->Load(gdal_dataset_path, color_path);
+
+ auto timer_callback = [this]() -> void {
+ auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap());
+ if (msg) {
+ msg->header.stamp = now();
+ original_map_pub_->publish(std::move(msg));
+ }
+ };
+ timer_ = this->create_wall_timer(5s, timer_callback);
+ ESPG epsg;
+ 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_);
+ }
+
+ private:
+ rclcpp::TimerBase::SharedPtr timer_;
+ rclcpp::Publisher::SharedPtr original_map_pub_;
+ std::shared_ptr map_;
+ std::shared_ptr tf_broadcaster_;
+};
+
+int main(int argc, char **argv) {
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}