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..29d91a3 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(); /** @@ -84,32 +85,40 @@ class GridMapGeo { }; /** - * @brief Set the Altitude Origin object - * - * @param altitude + * @brief Get the name of the coordinate frame of the dataset + * + * @return std::string */ - void setAltitudeOrigin(const double altitude) { localorigin_altitude_ = altitude; }; + std::string getCoordinateName() { return coordinate_name_; }; + + + /** + * @brief Overloading terrain loading with only elevation + * + * @param map_path Path to dsm path (Supported formats are *.tif) + */ + bool Load(const std::string& map_path) { + Load(map_path, ""); + } /** * @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 +180,12 @@ 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_{""}; + std::string coordinate_name_{""}; }; #endif diff --git a/launch/load_multiple_tif_launch.xml b/launch/load_multiple_tif_launch.xml new file mode 100644 index 0000000..d7be664 --- /dev/null +++ b/launch/load_multiple_tif_launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + 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/rviz/config.rviz b/rviz/config.rviz index 34ebd80..284125e 100644 --- a/rviz/config.rviz +++ b/rviz/config.rviz @@ -8,7 +8,7 @@ Panels: - /Status1 - /GridMap1 Splitter Ratio: 0.5 - Tree Height: 381 + Tree Height: 1106 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -64,7 +64,7 @@ Visualization Manager: Min Color: 0; 0; 0 Min Intensity: 0 Name: GridMap - Show Grid Lines: true + Show Grid Lines: false Topic: Depth: 5 Durability Policy: Volatile @@ -143,10 +143,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 670 + Height: 1403 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001630000021afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c0000021a000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000029afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c0000029a000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004280000003efc0100000002fb0000000800540069006d00650100000000000004280000023d00fffffffb0000000800540069006d00650100000000000004500000000000000000000002c40000021a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000163000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000029afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c0000029a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000897000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -155,6 +155,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1060 + Width: 2560 X: 0 Y: 0 diff --git a/rviz/multi_config.rviz b/rviz/multi_config.rviz new file mode 100644 index 0000000..2f51dd0 --- /dev/null +++ b/rviz/multi_config.rviz @@ -0,0 +1,187 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /GridMap1 + - /GridMap2 + Splitter Ratio: 0.5 + Tree Height: 1106 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 100 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: color + Color Transformer: ColorLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation2 + Use Rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: color + Color Transformer: ColorLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_map + Use Rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 22312.625 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -469.88262939453125 + Y: 4805.85498046875 + Z: -1997.9356689453125 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8752031326293945 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.6453869342803955 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1403 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000163000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000029afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c0000029a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000897000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2560 + X: 0 + Y: 0 diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp index 37cf227..a0d8249 100644 --- a/src/grid_map_geo.cpp +++ b/src/grid_map_geo.cpp @@ -58,12 +58,12 @@ #include #endif -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 +71,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 +102,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(); + coordinate_name_ = spatial_ref->GetAttrValue("geogcs"); // Get image metadata unsigned width = dataset->GetRasterXSize(); unsigned height = dataset->GetRasterYSize(); @@ -121,24 +123,8 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra Eigen::Vector2d position{Eigen::Vector2d::Zero()}; - /// 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; - // } - 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 +141,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..878137f 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -44,22 +44,27 @@ #include "grid_map_msgs/msg/grid_map.h" #include "grid_map_ros/GridMapRosConverter.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/static_transform_broadcaster.h" 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 color_path = this->declare_parameter("tif_color_path", "."); + std::string frame_id = this->declare_parameter("frame_id", "map"); + + original_map_pub_ = this->create_publisher("elevation_map", 1); RCLCPP_INFO_STREAM(get_logger(), "file_path " << file_path); RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path); + tf_broadcaster_ = std::make_shared(this); - map_ = std::make_shared(); - map_->Load(file_path, false, color_path); + map_ = std::make_shared(frame_id); + map_->Load(file_path, color_path); auto timer_callback = [this]() -> void { auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap()); if (msg) { @@ -68,12 +73,29 @@ class MapPublisher : public rclcpp::Node { } }; 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) {