Load multiple tif files

F
ethz-asl · Dec 2, 2023 · f0ce62e · f0ce62e
1 parent 2a88dae
commit f0ce62e
Show file tree

Hide file tree

Showing 5 changed files with 101 additions and 27 deletions.
diff --git a/launch/config.rviz b/launch/config.rviz
@@ -7,8 +7,12 @@ Panels:
         - /Global Options1
         - /Status1
         - /GridMap1
+        - /GridMap2
+        - /GridMap2/Status1
+        - /TF1
+        - /TF1/Status1
       Splitter Ratio: 0.5
-    Tree Height: 848
+    Tree Height: 839
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -24,7 +28,6 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: ""
@@ -59,11 +62,14 @@ Visualization Manager:
       Color: 200; 200; 200
       Color Layer: color
       Color Transformer: ColorLayer
+      ColorMap: default
       Enabled: true
+      Grid Cell Decimation: 1
+      Grid Line Thickness: 0.10000000149011612
       Height Layer: elevation
       Height Transformer: GridMapLayer
       History Length: 1
-      Invert Rainbow: false
+      Invert ColorMap: false
       Max Color: 255; 255; 255
       Max Intensity: 10
       Min Color: 0; 0; 0
@@ -72,13 +78,56 @@ Visualization Manager:
       Show Grid Lines: false
       Topic: /elevation_map
       Unreliable: false
-      Use Rainbow: true
+      Use ColorMap: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Class: grid_map_rviz_plugin/GridMap
+      Color: 200; 200; 200
+      Color Layer: elevation
+      Color Transformer: GridMapLayer
+      ColorMap: default
+      Enabled: true
+      Grid Cell Decimation: 1
+      Grid Line Thickness: 0.10000000149011612
+      Height Layer: elevation
+      Height Transformer: GridMapLayer
+      History Length: 1
+      Invert ColorMap: false
+      Max Color: 255; 255; 255
+      Max Intensity: 10
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: GridMap
+      Show Grid Lines: false
+      Topic: /elevation2
+      Unreliable: false
+      Use ColorMap: true
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        map:
+          Value: true
+        world:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
       Value: true
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
     Default Light: true
-    Fixed Frame: map
+    Fixed Frame: sertig
     Frame Rate: 30
   Name: root
   Tools:
@@ -102,33 +151,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2959.1279296875
+      Distance: 16350.146484375
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
-        X: 11.737425804138184
-        Y: -95.61576843261719
-        Z: 43.985958099365234
+        X: -428.4516906738281
+        Y: -385.00115966796875
+        Z: 529.86865234375
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8253984451293945
+      Pitch: 0.7153984904289246
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.6203981637954712
+      Yaw: 0.48539578914642334
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1145
+  Height: 1136
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000003dbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003db000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003dbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003db000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc000003db00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000283000003d2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000039a000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -137,6 +186,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1853
-  X: 67
+  Width: 1848
+  X: 72
   Y: 27
diff --git a/launch/load_multiple_tif.launch b/launch/load_multiple_tif.launch
@@ -0,0 +1,22 @@
+<launch>
+    <arg name="visualization" default="true"/>
+    <arg name="location" default="sargans"/>
+    <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/>
+
+    <node pkg="grid_map_geo" type="test_tif_loader" name="first_tif_loader" output="screen">
+        <param name="frame_id" value="duerrboden"/>
+        <param name="tif_path" value="$(find terrain_models)/models/duerrboden.tif"/>
+        <param name="color_path" value="$(find terrain_models)/models/duerrboden_color.tif"/>
+    </node>
+
+    <node pkg="grid_map_geo" type="test_tif_loader" name="second_tif_loader2" output="screen">
+        <param name="frame_id" value="dischma_valley"/>
+        <param name="tif_path" value="$(find terrain_models)/models/dischma_valley.tif"/>
+        <param name="color_path" value="$(find terrain_models)/models/dischma_valley_color.tif"/>
+        <param name="topic_name" value="elevation2"/>
+    </node>
+
+    <group if="$(arg visualization)">
+        <node type="rviz" name="rviz" pkg="rviz" args="-d $(find grid_map_geo)/launch/config.rviz" />
+    </group>
+</launch>
diff --git a/launch/load_tif.launch b/launch/load_tif.launch
@@ -4,8 +4,9 @@
     <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/>
 
     <node pkg="grid_map_geo" type="test_tif_loader" name="test_tif_loader" output="screen">
-        <param name="tif_path" value="$(find grid_map_geo)/resources/sertig.tif"/>
-        <!-- <param name="color_path" value="$(find grid_map_geo)/resources/sertig_color.tif"/> -->
+        <param name="frame_id" value="sertig"/>
+        <param name="tif_path" value="$(find terrain_models)/models/sertig.tif"/>
+        <param name="color_path" value="$(find terrain_models)/models/sertig_color.tif"/>
     </node>
 
     <group if="$(arg visualization)">

diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp
@@ -83,6 +83,8 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
   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();
@@ -125,15 +127,15 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
   geometry_msgs::TransformStamped static_transformStamped;
 
   static_transformStamped.header.stamp = ros::Time::now();
-  static_transformStamped.header.frame_id = "UTM";
+  static_transformStamped.header.frame_id = name_coordinate;
   static_transformStamped.child_frame_id = frame_id_;
-  static_transformStamped.transform.translation.x = 0.0;
-  static_transformStamped.transform.translation.y = 0.0;
+  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 = 0.0;
+  static_transformStamped.transform.rotation.w = 1.0;
   static_broadcaster.sendTransform(static_transformStamped);
 
   return true;

diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp
@@ -54,21 +54,21 @@ int main(int argc, char **argv) {
   ros::NodeHandle nh("");
   ros::NodeHandle nh_private("~");
 
-  ros::Publisher original_map_pub = nh.advertise<grid_map_msgs::GridMap>("elevation_map", 1, true);
-
-  std::string frame_id, file_path, color_path;
+  std::string frame_id, file_path, color_path, topic_name;
   nh_private.param<std::string>("frame_id", frame_id, "map");
   nh_private.param<std::string>("tif_path", file_path, "");
   nh_private.param<std::string>("color_path", color_path, "");
+  nh_private.param<std::string>("topic_name", topic_name, "elevation_map");
+
+  ros::Publisher original_map_pub = nh.advertise<grid_map_msgs::GridMap>(topic_name, 1, true);
 
   std::shared_ptr<GridMapGeo> map = std::make_shared<GridMapGeo>(frame_id);
   map->Load(file_path, color_path);
 
   while (true) {
     /// TODO: Publish gridmap
     MapPublishOnce(original_map_pub, map->getGridMap());
-    ros::Duration(10.0).sleep();
-    ros::Duration(3.0).sleep();
+    ros::Duration(1.0).sleep();
   }
 
   ros::spin();