Added DEM support to heightmaps

CMakeLists.txt

@@ -35,7 +35,7 @@ message(STATUS "\n\n-- ====== Finding Dependencies ======")
 #--------------------------------------
 # Find ignition-common
 ign_find_package(ignition-common5
-  COMPONENTS graphics profiler
+  COMPONENTS geospatial graphics profiler
   REQUIRED_BY heightmap mesh dartsim tpe tpelib bullet)
 set(IGN_COMMON_VER ${ignition-common5_VERSION_MAJOR})
 

dartsim/CMakeLists.txt

@@ -31,6 +31,7 @@ target_link_libraries(${dartsim_plugin}
     ${PROJECT_LIBRARY_TARGET_NAME}-heightmap
     ${PROJECT_LIBRARY_TARGET_NAME}-mesh
     ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+    ignition-common${IGN_COMMON_VER}::geospatial
     ignition-math${IGN_MATH_VER}::eigen3
   PRIVATE
     # We need to link this, even when the profiler isn't used to get headers.
@@ -84,6 +85,7 @@ ign_build_tests(
     ${features}
     ignition-plugin${IGN_PLUGIN_VER}::loader
     ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+    ignition-common${IGN_COMMON_VER}::geospatial
     ${PROJECT_LIBRARY_TARGET_NAME}-sdf
     ${PROJECT_LIBRARY_TARGET_NAME}-heightmap
     ${PROJECT_LIBRARY_TARGET_NAME}-mesh

dartsim/src/CustomHeightmapShape.cc

@@ -19,6 +19,7 @@
 
 #include <vector>
 #include <ignition/common/Console.hh>
+#include <ignition/common/Dem.hh>
 #include <ignition/common/ImageHeightmap.hh>
 #include <ignition/math/eigen3/Conversions.hh>
 
@@ -33,37 +34,52 @@ CustomHeightmapShape::CustomHeightmapShape(
     int _subSampling)
   : dart::dynamics::HeightmapShape<float>()
 {
-  double heightmapSizeZ = _input.MaxElevation();
+  float heightmapSizeZ = _input.MaxElevation();
   const bool flipY = false;
   const int vertSize = (_input.Width() * _subSampling) - _subSampling + 1;
+
   math::Vector3d scale;
   scale.X(_size(0) / vertSize);
   scale.Y(_size(1) / vertSize);
 
-  if (math::equal(heightmapSizeZ, 0.0))
+  if (math::equal(heightmapSizeZ, 0.0f))
     scale.Z(1.0);
   else
     scale.Z(fabs(_size(2)) / heightmapSizeZ);
 
   auto sizeIgn = ignition::math::eigen3::convert(_size);
 
-  // We need to make a copy here in order to use the non-const FillHeightMap
+  std::vector<float> heightsFloat;
+
+  // We need to make a copy below in order to use the non-const FillHeightMap
   // function
-  common::ImageHeightmap copyData;
-  try
+
+  // DEM
+  auto demData = dynamic_cast<const common::Dem *>(&_input);
+  if (demData)
   {
-    const auto &image = dynamic_cast<const common::ImageHeightmap &>(_input);
-    copyData.Load(image.Filename());
+    common::Dem copyData;
+    copyData.Load(_input.Filename());
+    copyData.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY,
+        heightsFloat);
   }
-  catch(const std::bad_cast &)
+  // Image
+  else
   {
-    ignerr << "Only image heightmaps are supported at the moment." << std::endl;
-    return;
-  }
+    common::ImageHeightmap copyData;
+    try
+    {
+      copyData.Load(_input.Filename());
+    }
+    catch(const std::bad_cast &)
+    {
+      ignerr << "Only DEM and image heightmaps are supported." << std::endl;
+      return;
+    }
 
-  std::vector<float> heightsFloat;
-  copyData.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY,
-      heightsFloat);
+    copyData.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY,
+        heightsFloat);
+  }
 
   this->setHeightField(vertSize, vertSize, heightsFloat);
   this->setScale(Vector3(scale.X(), scale.Y(), 1));

dartsim/src/EntityManagement_TEST.cc

@@ -19,6 +19,7 @@
 
 #include <ignition/plugin/Loader.hh>
 
+#include <ignition/common/Dem.hh>
 #include <ignition/common/ImageHeightmap.hh>
 #include <ignition/common/MeshManager.hh>
 #include <ignition/common/Filesystem.hh>
@@ -205,6 +206,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld)
   EXPECT_NEAR(meshShapeScaledSize[1], 0.3831, 1e-4);
   EXPECT_NEAR(meshShapeScaledSize[2], 0.0489, 1e-4);
 
+  // image heightmap
   auto heightmapLink = model->ConstructEmptyLink("heightmap_link");
   heightmapLink->AttachFixedJoint(child, "heightmap_joint");
 
@@ -230,6 +232,38 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld)
   EXPECT_NEAR(size.X(), heightmapShapeRecast->GetSize()[0], 1e-6);
   EXPECT_NEAR(size.Y(), heightmapShapeRecast->GetSize()[1], 1e-6);
   EXPECT_NEAR(size.Z(), heightmapShapeRecast->GetSize()[2], 1e-6);
+
+  //  dem heightmap
+  auto demLink = model->ConstructEmptyLink("dem_link");
+  demLink->AttachFixedJoint(child, "dem_joint");
+
+  auto demFilename = ignition::common::joinPaths(
+      IGNITION_PHYSICS_RESOURCE_DIR, "volcano.tif");
+  ignition::common::Dem dem;
+  EXPECT_EQ(0, dem.Load(demFilename));
+
+  ignition::math::Vector3d sizeDem;
+  sizeDem.X(dem.WorldWidth());
+  sizeDem.Y(dem.WorldHeight());
+  sizeDem.Z(dem.MaxElevation() - dem.MinElevation());
+
+  auto demShape = demLink->AttachHeightmapShape("dem", dem,
+      ignition::math::eigen3::convert(pose),
+      ignition::math::eigen3::convert(sizeDem));
+
+  // there is a loss in precision with large dems since heightmaps use floats
+  EXPECT_NEAR(sizeDem.X(), demShape->GetSize()[0], 1e-3);
+  EXPECT_NEAR(sizeDem.Y(), demShape->GetSize()[1], 1e-3);
+  EXPECT_NEAR(sizeDem.Z(), demShape->GetSize()[2], 1e-6);
+
+  auto demShapeGeneric = demLink->GetShape("dem");
+  ASSERT_NE(nullptr, demShapeGeneric);
+  EXPECT_EQ(nullptr, demShapeGeneric->CastToBoxShape());
+  auto demShapeRecast = demShapeGeneric->CastToHeightmapShape();
+  ASSERT_NE(nullptr, demShapeRecast);
+  EXPECT_NEAR(sizeDem.X(), demShapeRecast->GetSize()[0], 1e-3);
+  EXPECT_NEAR(sizeDem.Y(), demShapeRecast->GetSize()[1], 1e-3);
+  EXPECT_NEAR(sizeDem.Z(), demShapeRecast->GetSize()[2], 1e-6);
 }
 
 TEST(EntityManagement_TEST, RemoveEntities)

heightmap/CMakeLists.txt

@@ -3,7 +3,7 @@ ign_add_component(heightmap INTERFACE
 
 target_link_libraries(${heightmap}
   INTERFACE
-    ignition-common${IGN_COMMON_VER}::graphics)
+    ignition-common${IGN_COMMON_VER}::geospatial)
 
 install(
   DIRECTORY include/