From b8c0d1e89eab8eeba3e2a12014b929fe15a2ca60 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Wed, 10 Jan 2024 14:07:09 -0700 Subject: [PATCH] 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 --- CMakeLists.txt | 14 +++ README.md | 45 +++++++- config/sargans_color_over_srtm1.yaml | 8 ++ include/grid_map_geo/grid_map_geo.hpp | 35 ++++-- include/grid_map_geo/transform.hpp | 60 +++++++++- launch/load_map_publisher.launch.py | 18 +++ launch/load_map_publisher_launch.xml | 14 +++ launch/load_tif.launch.py | 50 ++++++--- launch/load_vrt_launch.xml | 21 ++++ src/grid_map_geo.cpp | 132 +++++++++++++++++----- src/map_publisher.cpp | 153 ++++++++++++++++++++++++++ 11 files changed, 491 insertions(+), 59 deletions(-) create mode 100644 config/sargans_color_over_srtm1.yaml create mode 100644 launch/load_map_publisher.launch.py create mode 100644 launch/load_map_publisher_launch.xml create mode 100644 launch/load_vrt_launch.xml create mode 100644 src/map_publisher.cpp 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; +}