Merge f299b26 into 5a5a974

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,7 +19,8 @@
 
 #include <vector>
 #include <ignition/common/Console.hh>
-#include <ignition/common/ImageHeightmap.hh>
+#include <ignition/common/geospatial/Dem.hh>
+#include <ignition/common/geospatial/ImageHeightmap.hh>
 #include <ignition/math/eigen3/Conversions.hh>
 
 namespace ignition {
@@ -33,36 +34,23 @@ CustomHeightmapShape::CustomHeightmapShape(
     int _subSampling)
   : dart::dynamics::HeightmapShape<float>()
 {
-  double heightmapSizeZ = _input.MaxElevation();
+  float heightmapSizeZ = _input.MaxElevation() - _input.MinElevation();
   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
-  // function
-  common::ImageHeightmap copyData;
-  try
-  {
-    const auto &image = dynamic_cast<const common::ImageHeightmap &>(_input);
-    copyData.Load(image.Filename());
-  }
-  catch(const std::bad_cast &)
-  {
-    ignerr << "Only image heightmaps are supported at the moment." << std::endl;
-    return;
-  }
-
   std::vector<float> heightsFloat;
-  copyData.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY,
+  _input.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY,
       heightsFloat);
 
   this->setHeightField(vertSize, vertSize, heightsFloat);

dartsim/src/CustomHeightmapShape.hh

@@ -19,7 +19,7 @@
 #define IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_
 
 #include <dart/dynamics/HeightmapShape.hpp>
-#include <ignition/common/HeightmapData.hh>
+#include <ignition/common/geospatial/HeightmapData.hh>
 
 namespace ignition {
 namespace physics {

dartsim/src/EntityManagement_TEST.cc

@@ -19,7 +19,8 @@
 
 #include <ignition/plugin/Loader.hh>
 
-#include <ignition/common/ImageHeightmap.hh>
+#include <ignition/common/geospatial/Dem.hh>
+#include <ignition/common/geospatial/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/

heightmap/include/ignition/physics/heightmap/HeightmapShape.hh

@@ -20,7 +20,7 @@
 
 #include <string>
 
-#include <ignition/common/HeightmapData.hh>
+#include <ignition/common/geospatial/HeightmapData.hh>
 
 #include <ignition/physics/DeclareShapeType.hh>
 #include <ignition/physics/Geometry.hh>