[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

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 GDAL::GDAL)
 ament_target_dependencies(${PROJECT_NAME} PUBLIC
   grid_map_core
   grid_map_ros
+  tf2_ros
 )
 
 add_executable(test_tif_loader

include/grid_map_geo/grid_map_geo.hpp

@@ -38,6 +38,7 @@
 
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
+#include "tf2_ros/transform_broadcaster.h"
 
 #if __APPLE__
 #include <cpl_string.h>
@@ -61,7 +62,7 @@ struct Location {
 
 class GridMapGeo {
  public:
-  GridMapGeo();
+  GridMapGeo(const std::string frame_id = "map");
   virtual ~GridMapGeo();
 
   /**
@@ -97,33 +98,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)
@@ -185,11 +177,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

launch/load_multiple_tif_launch.xml

@@ -0,0 +1,23 @@
+<launch>
+    <arg name="rviz" default="true"/>
+    <arg name="location" default="sargans"/>
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
+
+    <node pkg="grid_map_geo" exec="test_tif_loader" name="first_tif_loader" output="screen">
+        <param name="frame_id" value="map"/>
+        <param name="tif_path" value="/home/jaeyoung/dev/terrain-models/models/sertig.tif"/>
+        <param name="tif_color_path" value="/home/jaeyoung/dev/terrain-models/models/sertig_color.tif"/>
+    </node>
+
+    <node pkg="grid_map_geo" exec="test_tif_loader" name="second_tif_loader" output="screen">
+        <param name="frame_id" value="dischma_valley"/>
+        <param name="tif_path" value="/home/jaeyoung/dev/terrain-models/models/dischma_valley.tif"/>
+        <param name="tif_color_path" value="/home/jaeyoung/dev/terrain-models/models/dischma_valley_color.tif"/>
+        <param name="topic_name" value="elevation2"/>
+    </node>
+
+
+    <group if="$(var rviz)">
+        <node exec="rviz2" name="rviz2" pkg="rviz2"  args="-d $(find-pkg-share grid_map_geo)/rviz/config.rviz" />
+    </group>
+</launch>

launch/load_tif_launch.xml

@@ -4,11 +4,11 @@
     <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
 
     <node pkg="grid_map_geo" exec="test_tif_loader" name="test_tif_loader" output="screen">
-        <param name="tif_path" value="$(find-pkg-share grid_map_geo)/resources/sargans.tif"/>
-        <param name="tif_color_path" value="$(find-pkg-share grid_map_geo)/resources/sargans_color.tif"/>
+        <param name="tif_path" value="/home/jaeyoung/dev/terrain-models/models/sargans.tif"/>
+        <param name="tif_color_path" value="/home/jaeyoung/dev/terrain-models/models/sargans_color.tif"/>
     </node>
 
     <group if="$(var rviz)">
-        <node exec="rviz2" name="rviz2" pkg="rviz2"  args="-d $(find-pkg-share grid_map_geo)/launch/config.rviz" />
+        <node exec="rviz2" name="rviz2" pkg="rviz2"  args="-d $(find-pkg-share grid_map_geo)/rviz/config.rviz" />
     </group>
 </launch>

package.xml

@@ -16,6 +16,7 @@
   <depend>eigen</depend>
   <depend>grid_map_core</depend>
   <depend>grid_map_ros</depend>
+  <depend>tf2_ros</depend>
   <depend>libgdal-dev</depend>
   <depend>rclcpp</depend>
   <depend>yaml_cpp_vendor</depend>

src/grid_map_geo.cpp

@@ -43,20 +43,22 @@
 #include <grid_map_core/iterators/CircleIterator.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
 
-GridMapGeo::GridMapGeo() {}
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+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);
   }
   if (!loaded) return false;
   return true;
 }
 
-bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terrain) {
+bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
   GDALAllRegister();
   GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly);
   if (!dataset) {
@@ -80,6 +82,8 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra
   const char *pszProjection = dataset->GetProjectionRef();
   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();
@@ -97,26 +101,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);
 
@@ -133,17 +132,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;
 }
 

src/test_tif_loader.cpp

@@ -50,27 +50,34 @@ using namespace std::chrono_literals;
 class MapPublisher : public rclcpp::Node {
  public:
   MapPublisher() : Node("map_publisher") {
-    original_map_pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>("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<grid_map_msgs::msg::GridMap>(topic_name, 1);
 
     RCLCPP_INFO_STREAM(get_logger(), "file_path " << file_path);
     RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path);
 
-    map_ = std::make_shared<GridMapGeo>();
-    map_->Load(file_path, false, color_path);
+    map_ = std::make_shared<GridMapGeo>(frame_id);
+    map_->Load(file_path, color_path);
     timer_ = this->create_wall_timer(5s, std::bind(&MapPublisher::timer_callback, this));
   }
 
  private:
   void timer_callback() {
     auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap());
     original_map_pub_->publish(std::move(msg));
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+    map_->static_transformStamped.header.stamp = this->get_clock()->now();
+    tf_broadcaster_->sendTransform(map_->static_transformStamped);
   }
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr original_map_pub_;
   std::shared_ptr<GridMapGeo> map_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
 };
 
 int main(int argc, char **argv) {