diff --git a/CMakeLists.txt b/CMakeLists.txt index 986816bdc..87745cc59 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index 5e9572ce5..f5bfa8a0b 100644 --- a/dartsim/CMakeLists.txt +++ b/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 diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 3d5a287d6..97830f208 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -19,7 +19,8 @@ #include #include -#include +#include +#include #include namespace ignition { @@ -33,36 +34,23 @@ CustomHeightmapShape::CustomHeightmapShape( int _subSampling) : dart::dynamics::HeightmapShape() { - 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(_input); - copyData.Load(image.Filename()); - } - catch(const std::bad_cast &) - { - ignerr << "Only image heightmaps are supported at the moment." << std::endl; - return; - } - std::vector heightsFloat; - copyData.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY, + _input.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY, heightsFloat); this->setHeightField(vertSize, vertSize, heightsFloat); diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index 4c641ceec..0bebc4534 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -19,7 +19,7 @@ #define IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_ #include -#include +#include namespace ignition { namespace physics { diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index a97ddc7ac..d1830b2fd 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -19,7 +19,8 @@ #include -#include +#include +#include #include #include @@ -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) diff --git a/heightmap/CMakeLists.txt b/heightmap/CMakeLists.txt index 50efd33a0..0d31f22ae 100644 --- a/heightmap/CMakeLists.txt +++ b/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/ diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index 0f400181e..294c8d193 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -20,7 +20,7 @@ #include -#include +#include #include #include diff --git a/resources/volcano.tif b/resources/volcano.tif new file mode 100644 index 000000000..fa51cbc23 Binary files /dev/null and b/resources/volcano.tif differ