Heightmaps 🏔️ (gazebosim#218)

Signed-off-by: Steve Peters <[email protected]> Signed-off-by: Louise Poubel <[email protected]> Co-authored-by: Steve Peters <[email protected]>
vatanaksoytezer · Jul 14, 2021 · 1b64273 · 1b64273
1 parent 9d5cc9a
commit 1b64273
Show file tree

Hide file tree

Showing 14 changed files with 431 additions and 4 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -37,7 +37,7 @@ message(STATUS "\n\n-- ====== Finding Dependencies ======")
 # Find ignition-common
 ign_find_package(ignition-common4
   COMPONENTS graphics profiler
-  REQUIRED_BY mesh dartsim tpe tpelib)
+  REQUIRED_BY heightmap mesh dartsim tpe tpelib)
 set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR})
 
 #--------------------------------------
@@ -92,7 +92,7 @@ set(IGNITION_PHYSICS_ENGINE_INSTALL_DIR
 # Configure the build
 #============================================================================
 ign_configure_build(QUIT_IF_BUILD_ERRORS
-  COMPONENTS sdf mesh dartsim tpe)
+  COMPONENTS sdf heightmap mesh dartsim tpe)
 
 
 #============================================================================

diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt
@@ -2,7 +2,7 @@
 # This component expresses custom features of the dartsim plugin, which can
 # expose native dartsim data types.
 ign_add_component(dartsim INTERFACE
-  DEPENDS_ON_COMPONENTS sdf mesh
+  DEPENDS_ON_COMPONENTS sdf heightmap mesh
   GET_TARGET_NAME features)
 
 target_link_libraries(${features} INTERFACE ${DART_LIBRARIES})
@@ -29,6 +29,7 @@ target_link_libraries(${dartsim_plugin}
   PUBLIC
     ${features}
     ${PROJECT_LIBRARY_TARGET_NAME}-sdf
+    ${PROJECT_LIBRARY_TARGET_NAME}-heightmap
     ${PROJECT_LIBRARY_TARGET_NAME}-mesh
     ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
     ignition-math${IGN_MATH_VER}::eigen3
@@ -65,6 +66,7 @@ ign_build_tests(
     ignition-plugin${IGN_PLUGIN_VER}::loader
     ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
     ${PROJECT_LIBRARY_TARGET_NAME}-sdf
+    ${PROJECT_LIBRARY_TARGET_NAME}-heightmap
     ${PROJECT_LIBRARY_TARGET_NAME}-mesh
   TEST_LIST tests)
 

diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "CustomHeightmapShape.hh"
+
+#include <vector>
+#include <ignition/common/Console.hh>
+#include <ignition/common/ImageHeightmap.hh>
+#include <ignition/math/eigen3/Conversions.hh>
+
+namespace ignition {
+namespace physics {
+namespace dartsim {
+
+/////////////////////////////////////////////////
+CustomHeightmapShape::CustomHeightmapShape(
+    const common::HeightmapData &_input,
+    const Eigen::Vector3d &_size,
+    int _subSampling)
+  : dart::dynamics::HeightmapShape<float>()
+{
+  double 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))
+    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,
+      heightsFloat);
+
+  this->setHeightField(vertSize, vertSize, heightsFloat);
+  this->setScale(Vector3(scale.X(), scale.Y(), 1));
+}
+}
+}
+}
diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_
+#define IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_
+
+#include <dart/dynamics/HeightmapShape.hpp>
+#include <ignition/common/HeightmapData.hh>
+
+namespace ignition {
+namespace physics {
+namespace dartsim {
+
+/// \brief This class creates a custom derivative of dartsim's HeightmapShape
+/// class which allows an ignition::common::Heightmap to be converted into a
+/// HeightmapShape that can be used by dartsim.
+/// Using float precision because Bullet's collision detector doesn't support
+/// double. common::HeightmapData also holds floats.
+class CustomHeightmapShape : public dart::dynamics::HeightmapShape<float>
+{
+  /// \brief Constructor
+  /// \param[in] _input Holds heightmap data.
+  /// \param[in] _size Heightmap size in meters.
+  /// \param[in] _subSampling How much to subsample.
+  public: CustomHeightmapShape(
+      const common::HeightmapData &_input,
+      const Eigen::Vector3d &_size,
+      const int _subSampling);
+};
+}
+}
+}
+
+#endif  // IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_
diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc
@@ -19,7 +19,9 @@
 
 #include <ignition/plugin/Loader.hh>
 
+#include <ignition/common/ImageHeightmap.hh>
 #include <ignition/common/MeshManager.hh>
+#include <ignition/common/Filesystem.hh>
 
 #include <ignition/math/eigen3/Conversions.hh>
 
@@ -167,7 +169,8 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld)
   auto meshLink = model->ConstructEmptyLink("mesh_link");
   meshLink->AttachFixedJoint(child, "fixed");
 
-  const std::string meshFilename = IGNITION_PHYSICS_RESOURCE_DIR "/chassis.dae";
+  const std::string meshFilename = ignition::common::joinPaths(
+      IGNITION_PHYSICS_RESOURCE_DIR, "chassis.dae");
   auto &meshManager = *ignition::common::MeshManager::Instance();
   auto *mesh = meshManager.Load(meshFilename);
 
@@ -201,6 +204,32 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld)
   EXPECT_NEAR(meshShapeScaledSize[0], 0.2553, 1e-4);
   EXPECT_NEAR(meshShapeScaledSize[1], 0.3831, 1e-4);
   EXPECT_NEAR(meshShapeScaledSize[2], 0.0489, 1e-4);
+
+  auto heightmapLink = model->ConstructEmptyLink("heightmap_link");
+  heightmapLink->AttachFixedJoint(child, "heightmap_joint");
+
+  auto heightmapFilename = ignition::common::joinPaths(
+      IGNITION_PHYSICS_RESOURCE_DIR, "heightmap_bowl.png");
+  ignition::common::ImageHeightmap data;
+  EXPECT_EQ(0, data.Load(heightmapFilename));
+
+  const ignition::math::Vector3d size({129, 129, 10});
+  auto heightmapShape = heightmapLink->AttachHeightmapShape("heightmap", data,
+      ignition::math::eigen3::convert(pose),
+      ignition::math::eigen3::convert(size));
+
+  EXPECT_NEAR(size.X(), heightmapShape->GetSize()[0], 1e-6);
+  EXPECT_NEAR(size.Y(), heightmapShape->GetSize()[1], 1e-6);
+  EXPECT_NEAR(size.Z(), heightmapShape->GetSize()[2], 1e-6);
+
+  auto heightmapShapeGeneric = heightmapLink->GetShape("heightmap");
+  ASSERT_NE(nullptr, heightmapShapeGeneric);
+  EXPECT_EQ(nullptr, heightmapShapeGeneric->CastToBoxShape());
+  auto heightmapShapeRecast = heightmapShapeGeneric->CastToHeightmapShape();
+  ASSERT_NE(nullptr, heightmapShapeRecast);
+  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);
 }
 
 TEST(EntityManagement_TEST, RemoveEntities)

diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
@@ -28,6 +28,7 @@
 #include <dart/dynamics/CylinderShape.hpp>
 #include <dart/dynamics/EllipsoidShape.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
+#include <dart/dynamics/HeightmapShape.hpp>
 #include <dart/dynamics/MeshShape.hpp>
 #include <dart/dynamics/PlaneShape.hpp>
 #include <dart/dynamics/PrismaticJoint.hpp>
@@ -50,6 +51,7 @@
 #include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Geometry.hh>
+#include <sdf/Heightmap.hh>
 #include <sdf/Joint.hh>
 #include <sdf/JointAxis.hh>
 #include <sdf/Link.hh>
@@ -314,6 +316,15 @@ static ShapeAndTransform ConstructPlane(
           Eigen::Vector3d(planeDim, planeDim, planeDim)), tf};
 }
 
+/////////////////////////////////////////////////
+static ShapeAndTransform ConstructHeightmap(
+    const ::sdf::Heightmap & /*_heightmap*/)
+{
+  ignerr << "Heightmap construction from an SDF has not been implemented yet "
+         << "for dartsim.\n";
+  return {nullptr};
+}
+
 /////////////////////////////////////////////////
 static ShapeAndTransform ConstructMesh(
     const ::sdf::Mesh & /*_mesh*/)
@@ -362,6 +373,8 @@ static ShapeAndTransform ConstructGeometry(
     return ConstructPlane(*_geometry.PlaneShape());
   else if (_geometry.MeshShape())
     return ConstructMesh(*_geometry.MeshShape());
+  else if (_geometry.HeightmapShape())
+    return ConstructHeightmap(*_geometry.HeightmapShape());
 
   return {nullptr};
 }

diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc
@@ -23,6 +23,7 @@
 #include <dart/dynamics/CapsuleShape.hpp>
 #include <dart/dynamics/CylinderShape.hpp>
 #include <dart/dynamics/EllipsoidShape.hpp>
+#include <dart/dynamics/HeightmapShape.hpp>
 #include <dart/dynamics/MeshShape.hpp>
 #include <dart/dynamics/PlaneShape.hpp>
 #include <dart/dynamics/Shape.hpp>
@@ -31,6 +32,7 @@
 #include <ignition/common/Mesh.hh>
 #include <ignition/common/MeshManager.hh>
 
+#include "CustomHeightmapShape.hh"
 #include "CustomMeshShape.hh"
 
 namespace ignition {
@@ -323,6 +325,58 @@ Identity ShapeFeatures::AttachSphereShape(
   return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
 }
 
+//////////////////////////////////////////////////
+Identity ShapeFeatures::CastToHeightmapShape(
+    const Identity &_shapeID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_shapeID);
+
+  const dart::dynamics::ShapePtr &shape =
+      shapeInfo->node->getShape();
+
+  if (dynamic_cast<dart::dynamics::HeightmapShape<float> *>(shape.get()))
+    return this->GenerateIdentity(_shapeID, this->Reference(_shapeID));
+
+  return this->GenerateInvalidId();
+}
+
+/////////////////////////////////////////////////
+LinearVector3d ShapeFeatures::GetHeightmapShapeSize(
+    const Identity &_heightmapID) const
+{
+  const auto *shapeInfo = this->ReferenceInterface<ShapeInfo>(_heightmapID);
+
+  const auto *heightmap =
+      static_cast<dart::dynamics::HeightmapShape<float> *>(
+        shapeInfo->node->getShape().get());
+
+  return heightmap->getBoundingBox().getMax() -
+      heightmap->getBoundingBox().getMin();
+}
+
+/////////////////////////////////////////////////
+Identity ShapeFeatures::AttachHeightmapShape(
+    const Identity &_linkID,
+    const std::string &_name,
+    const common::HeightmapData &_heightmapData,
+    const Pose3d &_pose,
+    const LinearVector3d &_size,
+    int _subSampling)
+{
+  auto heightmap = std::make_shared<CustomHeightmapShape>(_heightmapData,
+      _size, _subSampling);
+
+  DartBodyNode *bn = this->ReferenceInterface<LinkInfo>(_linkID)->link.get();
+  dart::dynamics::ShapeNode *sn =
+      bn->createShapeNodeWith<dart::dynamics::CollisionAspect,
+                              dart::dynamics::DynamicsAspect>(
+          heightmap, bn->getName() + ":" + _name);
+
+  sn->setRelativeTransform(_pose);
+  const std::size_t shapeID = this->AddShape({sn, _name});
+  return this->GenerateIdentity(shapeID, this->shapes.at(shapeID));
+}
+
 /////////////////////////////////////////////////
 Identity ShapeFeatures::CastToMeshShape(
     const Identity &_shapeID) const

diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh
@@ -25,6 +25,7 @@
 #include <ignition/physics/CapsuleShape.hh>
 #include <ignition/physics/CylinderShape.hh>
 #include <ignition/physics/EllipsoidShape.hh>
+#include <ignition/physics/heightmap/HeightmapShape.hh>
 #include <ignition/physics/mesh/MeshShape.hh>
 #include <ignition/physics/PlaneShape.hh>
 #include <ignition/physics/SphereShape.hh>
@@ -66,6 +67,10 @@ struct ShapeFeatureList : FeatureList<
 //  SetSphereShapeProperties,
   AttachSphereShapeFeature,
 
+  heightmap::GetHeightmapShapeProperties,
+//  heightmap::SetHeightmapShapeProperties,
+  heightmap::AttachHeightmapShapeFeature,
+
   mesh::GetMeshShapeProperties,
 //  mesh::SetMeshShapeProperties,
   mesh::AttachMeshShapeFeature,
@@ -160,6 +165,21 @@ class ShapeFeatures :
       const Pose3d &_pose) override;
 
 
+  // ----- Heightmap Features -----
+  public: Identity CastToHeightmapShape(
+      const Identity &_shapeID) const override;
+
+  public: LinearVector3d GetHeightmapShapeSize(
+      const Identity &_heightmapID) const override;
+
+  public: Identity AttachHeightmapShape(
+      const Identity &_linkID,
+      const std::string &_name,
+      const common::HeightmapData &_heightmapData,
+      const Pose3d &_pose,
+      const LinearVector3d &_size,
+      int _subSampling) override;
+
   // ----- Mesh Features -----
   public: Identity CastToMeshShape(
       const Identity &_shapeID) const override;

diff --git a/heightmap/CMakeLists.txt b/heightmap/CMakeLists.txt
@@ -0,0 +1,10 @@
+ign_add_component(heightmap INTERFACE
+  GET_TARGET_NAME heightmap)
+
+target_link_libraries(${heightmap}
+  INTERFACE
+    ignition-common${IGN_COMMON_VER}::graphics)
+
+install(
+  DIRECTORY include/
+  DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}")