From 46fd31dbbc533b09146579bbbf859dac3d5958c6 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Sat, 30 Nov 2024 16:18:03 -0700 Subject: [PATCH] Port terrain loader to ROS 2 * Needs a node instance to get the time * Move rviz config to the rviz directory * Use new grid map shared pointer API for converting to ROS messages * Install new node in the ament index Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- README.md | 2 +- grid_map_geo/CMakeLists.txt | 3 +- .../include/grid_map_geo/grid_map_geo.hpp | 4 +- ...r.launch => run_terrain_loader.launch.xml} | 6 +-- .../{launch => rviz}/terrain_loader.rviz | 0 grid_map_geo/src/grid_map_geo.cpp | 8 ++-- grid_map_geo/src/terrain_loader.cpp | 47 +++++++++++-------- 7 files changed, 39 insertions(+), 31 deletions(-) rename grid_map_geo/launch/{run_terrain_loader.launch => run_terrain_loader.launch.xml} (62%) rename grid_map_geo/{launch => rviz}/terrain_loader.rviz (100%) diff --git a/README.md b/README.md index 479a3ba..b9644d3 100644 --- a/README.md +++ b/README.md @@ -77,7 +77,7 @@ ros2 launch grid_map_geo load_tif_launch.xml One can load a DEM from a terrain server directly. ``` -roslaunch grid_map_geo run_terrain_loader.launch +ros2 launch grid_map_geo run_terrain_loader.launch.xml ``` ![terrain-loader](https://github.com/ethz-asl/grid_map_geo/assets/5248102/e93b2c86-c26a-477c-8704-dc0233b7ef2e) diff --git a/grid_map_geo/CMakeLists.txt b/grid_map_geo/CMakeLists.txt index 308aa46..78f4759 100644 --- a/grid_map_geo/CMakeLists.txt +++ b/grid_map_geo/CMakeLists.txt @@ -96,7 +96,8 @@ ament_export_dependencies(Eigen3 GDAL grid_map_core grid_map_msgs grid_map_ros t install( TARGETS - test_tif_loader + test_tif_loader + terrain_loader DESTINATION lib/${PROJECT_NAME} ) diff --git a/grid_map_geo/include/grid_map_geo/grid_map_geo.hpp b/grid_map_geo/include/grid_map_geo/grid_map_geo.hpp index 4754850..8d39071 100644 --- a/grid_map_geo/include/grid_map_geo/grid_map_geo.hpp +++ b/grid_map_geo/include/grid_map_geo/grid_map_geo.hpp @@ -34,7 +34,7 @@ #ifndef GRID_MAP_GEO_H #define GRID_MAP_GEO_H -#include +#include #include #include @@ -117,7 +117,7 @@ class GridMapGeo { */ bool initializeFromGeotiff(const std::string& path); - bool initializeFromVrt(const std::string& path, const Eigen::Vector2d& map_center, Eigen::Vector2d& extent); + bool initializeFromVrt(const std::string& path, const Eigen::Vector2d& map_center, Eigen::Vector2d& extent, rclcpp::Node::SharedPtr node_ptr); /** * @brief Load a color layer from a geotiff file (orthomosaic) diff --git a/grid_map_geo/launch/run_terrain_loader.launch b/grid_map_geo/launch/run_terrain_loader.launch.xml similarity index 62% rename from grid_map_geo/launch/run_terrain_loader.launch rename to grid_map_geo/launch/run_terrain_loader.launch.xml index 4ab95a4..2d695ea 100644 --- a/grid_map_geo/launch/run_terrain_loader.launch +++ b/grid_map_geo/launch/run_terrain_loader.launch.xml @@ -2,12 +2,12 @@ - + - - + + diff --git a/grid_map_geo/launch/terrain_loader.rviz b/grid_map_geo/rviz/terrain_loader.rviz similarity index 100% rename from grid_map_geo/launch/terrain_loader.rviz rename to grid_map_geo/rviz/terrain_loader.rviz diff --git a/grid_map_geo/src/grid_map_geo.cpp b/grid_map_geo/src/grid_map_geo.cpp index 4c9e876..b4d51b1 100644 --- a/grid_map_geo/src/grid_map_geo.cpp +++ b/grid_map_geo/src/grid_map_geo.cpp @@ -145,7 +145,7 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) { } bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2d &map_center, - Eigen::Vector2d &extent) { + Eigen::Vector2d &extent, rclcpp::Node::SharedPtr node_ptr) { GDALAllRegister(); GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly); if (!dataset) { @@ -223,10 +223,10 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2 layer_elevation(x, y) = data[gridMapIndex(0) + grid_width * gridMapIndex(1)]; } - static tf2_ros::StaticTransformBroadcaster static_broadcaster; - geometry_msgs::TransformStamped static_transformStamped; + static tf2_ros::StaticTransformBroadcaster static_broadcaster(node_ptr); + geometry_msgs::msg::TransformStamped static_transformStamped; - static_transformStamped.header.stamp = ros::Time::now(); + static_transformStamped.header.stamp = node_ptr->now(); static_transformStamped.header.frame_id = name_coordinate; static_transformStamped.child_frame_id = frame_id_; static_transformStamped.transform.translation.x = map_center(0); diff --git a/grid_map_geo/src/terrain_loader.cpp b/grid_map_geo/src/terrain_loader.cpp index d1f75c4..3c44893 100644 --- a/grid_map_geo/src/terrain_loader.cpp +++ b/grid_map_geo/src/terrain_loader.cpp @@ -37,12 +37,13 @@ * @author Jaeyoung Lim */ -#include -#include -#include +#include +#include +#include #include #include +#include constexpr int ESPG_WGS84 = 4326; constexpr int ESPG_CH1903_LV03 = 21781; @@ -67,7 +68,8 @@ void transformCoordinates(int src_coord, const double &x, const double &y, const } void LoadTerrainFromVrt(std::string path, const Eigen::Vector3d &query_position, const Eigen::Vector2d &offset, - grid_map::GridMap &map, grid_map::GridMap &vrt_map_object) { + grid_map::GridMap &map, grid_map::GridMap &vrt_map_object, + rclcpp::Node::SharedPtr node_ptr) { std::shared_ptr vrt_map = std::make_shared(); Eigen::Vector3d query_position_lv03 = query_position; @@ -80,7 +82,7 @@ void LoadTerrainFromVrt(std::string path, const Eigen::Vector3d &query_position, std::cout << " - map_center_wgs84:" << map_center_wgs84.transpose() << std::endl; /// TODO: Discover extent from corners Eigen::Vector2d extent_wgs84(0.5, 0.5); - vrt_map->initializeFromVrt(path, map_center_wgs84.head(2), extent_wgs84); + vrt_map->initializeFromVrt(path, map_center_wgs84.head(2), extent_wgs84, node_ptr); std::cout << " - Success!" << std::endl; /// TODO: Loaded VRT map @@ -115,15 +117,16 @@ void LoadTerrainFromVrt(std::string path, const Eigen::Vector3d &query_position, } int main(int argc, char **argv) { - ros::init(argc, argv, "terrain_loader"); - ros::NodeHandle nh(""); - ros::NodeHandle nh_private("~"); + rclcpp::init(argc, argv); + // ros::init(argc, argv, "terrain_loader"); + auto node = rclcpp::Node::make_shared("terrain_loader"); + // ros::NodeHandle nh(""); + // ros::NodeHandle nh_private("~"); - ros::Publisher map_pub = nh.advertise("grid_map", 1, true); - ros::Publisher map_pub2 = nh.advertise("grid_map2", 1, true); + auto map_pub = node->create_publisher("grid_map", rclcpp::QoS(1).transient_local()); + auto map_pub2 = node->create_publisher("grid_map2", rclcpp::QoS(1).transient_local()); - std::string path; - nh_private.param("terrain_path", path, "resources/cadastre.tif"); + auto const path = node->declare_parameter("terrain_path", "resources/cadastre.tif"); Eigen::Vector3d test_position = Eigen::Vector3d(783199.15, 187585.10, 0.0); @@ -133,18 +136,22 @@ int main(int argc, char **argv) { grid_map::GridMap map; grid_map::GridMap vrt_map; - LoadTerrainFromVrt(path, query_position, offset.head(2), map, vrt_map); + LoadTerrainFromVrt(path, query_position, offset.head(2), map, vrt_map, node); std::cout << "i: " << i << " offset: " << offset.transpose() << std::endl; - grid_map_msgs::GridMap msg; - grid_map::GridMapRosConverter::toMessage(map, msg); - map_pub.publish(msg); + // grid_map_msgs::msg::GridMap msg; + auto msg = grid_map::GridMapRosConverter::toMessage(map); + if(msg) { + map_pub->publish(std::move(msg)); + } - grid_map_msgs::GridMap msg2; - grid_map::GridMapRosConverter::toMessage(vrt_map, msg2); - map_pub2.publish(msg2); + auto msg2 = grid_map::GridMapRosConverter::toMessage(vrt_map); + if (msg2) { + map_pub2->publish(std::move(msg2)); + } } - ros::spin(); + rclcpp::spin(node); + rclcpp::shutdown(); return 0; }