[Backport] Align multiple DEMs with TF (#34) (#36)

* [Backport] Align multiple DEMs with TF (#34)

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

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"
 
 #include <iostream>
 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

launch/load_multiple_tif_launch.xml

@@ -0,0 +1,22 @@
+<launch>
+    <arg name="rviz" default="true"/>
+    <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="$(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"/>
+    </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="$(find-pkg-share grid_map_geo)/resources/dischma_valley.tif"/>
+        <param name="tif_color_path" value="$(find-pkg-share grid_map_geo)/resources/dischma_valley_color.tif"/>
+        <remap from="elevation_map" to="second_elevation_map"/>
+    </node>
+
+
+    <group if="$(var rviz)">
+        <node exec="rviz2" name="rviz2" pkg="rviz2"  args="-d $(find-pkg-share grid_map_geo)/rviz/multi_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>

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

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: <Fixed 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: <Fixed 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