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 <[email protected]>

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)

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}
 )
 

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 <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
 
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
@@ -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)

grid_map_geo/launch/run_terrain_loader.launch → grid_map_geo/launch/run_terrain_loader.launch.xml

@@ -2,12 +2,12 @@
     <arg name="visualization" default="true"/>
     <arg name="location" default="hinwil"/>
 
-    <node pkg="grid_map_geo" type="terrain_loader" name="terrain_loader" output="screen">
+    <node pkg="grid_map_geo" exec="terrain_loader" name="terrain_loader" output="screen">
         <!-- <param name="terrain_path" value="$(find terrain_models)/models/$(arg location).tif"/> -->
         <param name="terrain_path" value="/vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/ap_srtm1.zip/ap_srtm1.vrt"/>
     </node>
 
-    <group if="$(arg visualization)">
-        <node type="rviz" name="rviz" pkg="rviz" args="-d $(find grid_map_geo)/launch/terrain_loader.rviz" output="screen"/>
+    <group if="$(var visualization)">
+        <node exec="rviz2" name="rviz2" pkg="rviz2" args="-d $(find-pkg-share grid_map_geo)/rviz/terrain_loader.rviz" output="screen"/>
     </group>
 </launch>

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);

grid_map_geo/src/terrain_loader.cpp

@@ -37,12 +37,13 @@
  * @author Jaeyoung Lim <[email protected]>
  */
 
-#include <grid_map_geo/grid_map_geo.h>
-#include <grid_map_msgs/GridMap.h>
-#include <ros/ros.h>
+#include <grid_map_geo/grid_map_geo.hpp>
+#include <grid_map_msgs/msg/grid_map.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <grid_map_ros/GridMapRosConverter.hpp>
 
 #include <gdal/ogr_spatialref.h>
+#include <gdal/ogr_geometry.h>
 
 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<GridMapGeo> vrt_map = std::make_shared<GridMapGeo>();
 
   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_msgs::GridMap>("grid_map", 1, true);
-  ros::Publisher map_pub2 = nh.advertise<grid_map_msgs::GridMap>("grid_map2", 1, true);
+  auto map_pub = node->create_publisher<grid_map_msgs::msg::GridMap>("grid_map", rclcpp::QoS(1).transient_local());
+  auto map_pub2 = node->create_publisher<grid_map_msgs::msg::GridMap>("grid_map2", rclcpp::QoS(1).transient_local());
 
-  std::string path;
-  nh_private.param<std::string>("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;
 }