From e6df83acaa9b0f36a3087fbdddfe9d941ecfdc04 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Thu, 11 Jan 2024 11:02:44 +0100 Subject: [PATCH] [Backport] Align multiple DEMs with TF (#34) * Remove terrain alignment * Make frame_id a member of GridMapGeo * Broadcast Tf * Load multiple tif files Backport WIP transform stamped Fix path to rviz config F --- CMakeLists.txt | 2 ++ include/grid_map_geo/grid_map_geo.hpp | 22 ++++-------- launch/load_multiple_tif_launch.xml | 23 +++++++++++++ package.xml | 1 + src/grid_map_geo.cpp | 49 +++++++++++---------------- src/test_tif_loader.cpp | 7 ++-- 6 files changed, 57 insertions(+), 47 deletions(-) create mode 100644 launch/load_multiple_tif_launch.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index febfa56..53f6666 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(ament_cmake REQUIRED) find_package(grid_map_msgs REQUIRED) find_package(grid_map_ros REQUIRED) find_package(grid_map_core REQUIRED) +find_package(tf2_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(Eigen3 REQUIRED) @@ -49,6 +50,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen PRIVATE GDAL::GDAL) ament_target_dependencies(${PROJECT_NAME} PUBLIC grid_map_core grid_map_ros + tf2_ros ) add_executable(test_tif_loader diff --git a/include/grid_map_geo/grid_map_geo.hpp b/include/grid_map_geo/grid_map_geo.hpp index 02d9122..c094ddc 100644 --- a/include/grid_map_geo/grid_map_geo.hpp +++ b/include/grid_map_geo/grid_map_geo.hpp @@ -38,6 +38,7 @@ #include #include +#include "tf2_ros/transform_broadcaster.h" #include struct Location { @@ -47,7 +48,7 @@ struct Location { class GridMapGeo { public: - GridMapGeo(); + GridMapGeo(const std::string frame_id = "map"); virtual ~GridMapGeo(); /** @@ -83,33 +84,24 @@ class GridMapGeo { origin = maporigin_.position; }; - /** - * @brief Set the Altitude Origin object - * - * @param altitude - */ - void setAltitudeOrigin(const double altitude) { localorigin_altitude_ = altitude; }; - /** * @brief Helper function for loading terrain from path * * @param map_path Path to dsm path (Supported formats are *.tif) - * @param algin_terrain Geo align terrain * @param color_map_path Path to color raster files to visualize terrain texture (Supported formats are *.tif) * @return true Successfully loaded terrain * @return false Failed to load terrain */ - bool Load(const std::string& map_path, bool algin_terrain, 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 * * @param path Path to dsm path (Supported formats are *.tif) - * @param align_terrain * @return true Successfully loaded terrain * @return false Failed to load terrain */ - bool initializeFromGeotiff(const std::string& path, bool align_terrain = true); + bool initializeFromGeotiff(const std::string& path); /** * @brief Load a color layer from a geotiff file (orthomosaic) @@ -171,11 +163,11 @@ class GridMapGeo { */ void AddLayerNormals(std::string reference_layer); + geometry_msgs::msg::TransformStamped static_transformStamped; + protected: grid_map::GridMap grid_map_; - double localorigin_e_{789823.93}; // duerrboden berghaus - double localorigin_n_{177416.56}; - double localorigin_altitude_{0.0}; Location maporigin_; + std::string frame_id_{""}; }; #endif diff --git a/launch/load_multiple_tif_launch.xml b/launch/load_multiple_tif_launch.xml new file mode 100644 index 0000000..daef4aa --- /dev/null +++ b/launch/load_multiple_tif_launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index ac5463e..2654b18 100644 --- a/package.xml +++ b/package.xml @@ -16,6 +16,7 @@ eigen grid_map_core grid_map_ros + tf2_ros libgdal-dev rclcpp yaml_cpp_vendor diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp index 37cf227..4ba2022 100644 --- a/src/grid_map_geo.cpp +++ b/src/grid_map_geo.cpp @@ -44,6 +44,7 @@ #include #include +#include "geometry_msgs/msg/transform_stamped.hpp" #if __APPLE__ #include #include @@ -60,10 +61,12 @@ GridMapGeo::GridMapGeo() {} +GridMapGeo::GridMapGeo(const std::string frame_id) { frame_id_ = frame_id; } + GridMapGeo::~GridMapGeo() {} -bool GridMapGeo::Load(const std::string &map_path, bool algin_terrain, const std::string color_map_path) { - bool loaded = initializeFromGeotiff(map_path, algin_terrain); +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 bool color_loaded = addColorFromGeotiff(color_map_path); } @@ -71,7 +74,7 @@ bool GridMapGeo::Load(const std::string &map_path, bool algin_terrain, const std return true; } -bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terrain) { +bool GridMapGeo::initializeFromGeotiff(const std::string &path) { GDALAllRegister(); const auto dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(GDALOpen(path.c_str(), GA_ReadOnly))); if (!dataset) { @@ -102,6 +105,8 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra std::cout << std::endl << "Wkt ProjectionRef: " << pszProjection << std::endl; + const OGRSpatialReference *spatial_ref = dataset->GetSpatialRef(); + std::string name_coordinate = spatial_ref->GetAttrValue("geogcs"); // Get image metadata unsigned width = dataset->GetRasterXSize(); unsigned height = dataset->GetRasterYSize(); @@ -119,26 +124,21 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra maporigin_.espg = ESPG::CH1903_LV03; maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0); - Eigen::Vector2d position{Eigen::Vector2d::Zero()}; + static_transformStamped.header.frame_id = name_coordinate; + static_transformStamped.child_frame_id = frame_id_; + static_transformStamped.transform.translation.x = mapcenter_e; + static_transformStamped.transform.translation.y = mapcenter_n; + 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; - /// TODO: Generalize to set local origin as center of map position - // Eigen::Vector3d origin_lv03 = - // transformCoordinates(ESPG::WGS84, std::string(pszProjection), localorigin_wgs84_.position); - // localorigin_e_ = origin_lv03(0); - // localorigin_n_ = origin_lv03(1); - // localorigin_altitude_ = origin_lv03(2); - // if (align_terrain) { - // std::cout << "[GridMapGeo] Aligning terrain!" << std::endl; - // double map_position_x = mapcenter_e - localorigin_e_; - // double map_position_y = mapcenter_n - localorigin_n_; - // position = Eigen::Vector2d(map_position_x, map_position_y); - // } else { - // std::cout << "[GridMapGeo] Not aligning terrain!" << std::endl; - // } + Eigen::Vector2d position{Eigen::Vector2d::Zero()}; grid_map_.setGeometry(length, resolution, position); /// TODO: Use TF for geocoordinates - grid_map_.setFrameId("map"); + grid_map_.setFrameId(frame_id_); grid_map_.add("elevation"); GDALRasterBand *elevationBand = dataset->GetRasterBand(1); @@ -155,17 +155,6 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra layer_elevation(x, y) = data[gridMapIndex(0) + width * gridMapIndex(1)]; } - /// TODO: This is a workaround with the problem of gdal 3 not translating altitude correctly. - /// This section just levels the current position to the ground - double altitude{0.0}; - if (grid_map_.isInside(Eigen::Vector2d(0.0, 0.0))) { - altitude = grid_map_.atPosition("elevation", Eigen::Vector2d(0.0, 0.0)); - } - - // Eigen::Translation3d meshlab_translation(0.0, 0.0, -altitude); - // Eigen::AngleAxisd meshlab_rotation(Eigen::AngleAxisd::Identity()); - // Eigen::Isometry3d transform = meshlab_translation * meshlab_rotation; // Apply affine transformation. - // grid_map_ = grid_map_.getTransformedMap(transform, "elevation", grid_map_.getFrameId(), true); return true; } diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp index 98ab52c..28e131d 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -50,10 +50,12 @@ using namespace std::chrono_literals; class MapPublisher : public rclcpp::Node { public: MapPublisher() : Node("map_publisher") { - original_map_pub_ = this->create_publisher("elevation_map", 1); std::string file_path = this->declare_parameter("tif_path", "."); - std::string color_path = this->declare_parameter("tif_color_path", ""); + std::string frame_id = this->declare_parameter("frame_id", "map"); + std::string topic_name = this->declare_parameter("topic_name", "elevation_map"); + + original_map_pub_ = this->create_publisher(topic_name, 1); RCLCPP_INFO_STREAM(get_logger(), "file_path " << file_path); RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path); @@ -74,6 +76,7 @@ class MapPublisher : public rclcpp::Node { rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr original_map_pub_; std::shared_ptr map_; + std::unique_ptr tf_broadcaster_; }; int main(int argc, char **argv) {