Support visualizing mesh collisions with convex decomposition (#2352)

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Jun 25, 2024 · 3487086 · 3487086
1 parent e116d0b
commit 3487086
Show file tree

Hide file tree

Showing 4 changed files with 106 additions and 10 deletions.
diff --git a/src/Conversions.cc b/src/Conversions.cc
@@ -220,6 +220,24 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in)
     meshMsg->set_filename(asFullPath(meshSdf->Uri(), meshSdf->FilePath()));
     meshMsg->set_submesh(meshSdf->Submesh());
     meshMsg->set_center_submesh(meshSdf->CenterSubmesh());
+
+    if (!meshSdf->OptimizationStr().empty())
+    {
+      auto header = out.mutable_header()->add_data();
+      header->set_key("optimization");
+      header->add_value(meshSdf->OptimizationStr());
+    }
+    if (meshSdf->ConvexDecomposition())
+    {
+      auto header = out.mutable_header()->add_data();
+      header->set_key("max_convex_hulls");
+      header->add_value(std::to_string(
+          meshSdf->ConvexDecomposition()->MaxConvexHulls()));
+      header = out.mutable_header()->add_data();
+      header->set_key("voxel_resolution");
+      header->add_value(std::to_string(
+          meshSdf->ConvexDecomposition()->VoxelResolution()));
+    }
   }
   else if (_in.Type() == sdf::GeometryType::HEIGHTMAP && _in.HeightmapShape())
   {
@@ -359,6 +377,24 @@ sdf::Geometry gz::sim::convert(const msgs::Geometry &_in)
     meshShape.SetSubmesh(_in.mesh().submesh());
     meshShape.SetCenterSubmesh(_in.mesh().center_submesh());
 
+    sdf::ConvexDecomposition convexDecomp;
+    for (int i = 0; i < _in.header().data_size(); ++i)
+    {
+      auto data = _in.header().data(i);
+      if (data.key() == "optimization" && data.value_size() > 0)
+      {
+        meshShape.SetOptimization(data.value(0));
+      }
+      if (data.key() == "max_convex_hulls" && data.value_size() > 0)
+      {
+        convexDecomp.SetMaxConvexHulls(std::stoul(data.value(0)));
+      }
+      if (data.key() == "voxel_resolution" && data.value_size() > 0)
+      {
+        convexDecomp.SetVoxelResolution(std::stoul(data.value(0)));
+      }
+    }
+    meshShape.SetConvexDecomposition(convexDecomp);
     out.SetMeshShape(meshShape);
   }
   else if (_in.type() == msgs::Geometry::HEIGHTMAP && _in.has_heightmap())

diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc
@@ -463,6 +463,11 @@ TEST(Conversions, GeometryMesh)
   meshShape.SetUri("file://watermelon");
   meshShape.SetSubmesh("grape");
   meshShape.SetCenterSubmesh(true);
+  meshShape.SetOptimization("convex_decomposition");
+  sdf::ConvexDecomposition convexDecomp;
+  convexDecomp.SetMaxConvexHulls(4);
+  convexDecomp.SetVoxelResolution(10000);
+  meshShape.SetConvexDecomposition(convexDecomp);
   geometry.SetMeshShape(meshShape);
 
   auto geometryMsg = convert<msgs::Geometry>(geometry);
@@ -473,6 +478,15 @@ TEST(Conversions, GeometryMesh)
   EXPECT_EQ("file://watermelon", geometryMsg.mesh().filename());
   EXPECT_EQ("grape", geometryMsg.mesh().submesh());
   EXPECT_TRUE(geometryMsg.mesh().center_submesh());
+  auto header = geometryMsg.header().data(0);
+  EXPECT_EQ("optimization", header.key());
+  EXPECT_EQ("convex_decomposition", header.value(0));
+  header = geometryMsg.header().data(1);
+  EXPECT_EQ("max_convex_hulls", header.key());
+  EXPECT_EQ("4", header.value(0));
+  header = geometryMsg.header().data(2);
+  EXPECT_EQ("voxel_resolution", header.key());
+  EXPECT_EQ("10000", header.value(0));
 
   auto newGeometry = convert<sdf::Geometry>(geometryMsg);
   EXPECT_EQ(sdf::GeometryType::MESH, newGeometry.Type());
@@ -481,6 +495,11 @@ TEST(Conversions, GeometryMesh)
   EXPECT_EQ("file://watermelon", newGeometry.MeshShape()->Uri());
   EXPECT_EQ("grape", newGeometry.MeshShape()->Submesh());
   EXPECT_TRUE(newGeometry.MeshShape()->CenterSubmesh());
+  EXPECT_EQ("convex_decomposition", newGeometry.MeshShape()->OptimizationStr());
+  auto newConvexDecomp = newGeometry.MeshShape()->ConvexDecomposition();
+  ASSERT_NE(nullptr, newConvexDecomp);
+  EXPECT_EQ(4, newConvexDecomp->MaxConvexHulls());
+  EXPECT_EQ(10000, newConvexDecomp->VoxelResolution());
 }
 
 /////////////////////////////////////////////////

diff --git a/src/Util.cc b/src/Util.cc
@@ -891,32 +891,60 @@ const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf,
 
   auto &meshManager = *common::MeshManager::Instance();
   std::size_t maxConvexHulls = 16u;
+  std::size_t voxelResolution = 200000u;
   if (_meshSdf.Optimization() == sdf::MeshOptimization::CONVEX_HULL)
   {
     /// create 1 convex hull for the whole submesh
     maxConvexHulls = 1u;
   }
   else if (_meshSdf.ConvexDecomposition())
   {
-    // limit max number of convex hulls to generate
+    // set convex decomposition params: max number of convex hulls
+    // and voxel resolution
     maxConvexHulls = _meshSdf.ConvexDecomposition()->MaxConvexHulls();
+    voxelResolution = _meshSdf.ConvexDecomposition()->VoxelResolution();
   }
   // Check if MeshManager contains the decomposed mesh already. If not
   // add it to the MeshManager so we do not need to decompose it again.
   const std::string convexMeshName =
-      _mesh.Name() + "_CONVEX_" + std::to_string(maxConvexHulls);
+      _mesh.Name() + "_" + _meshSdf.Submesh() + "_CONVEX_" +
+      std::to_string(maxConvexHulls) + "_" + std::to_string(voxelResolution);
   auto *optimizedMesh = meshManager.MeshByName(convexMeshName);
   if (!optimizedMesh)
   {
-    // Merge meshes before convex decomposition
-    auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(_mesh);
-    if (mergedMesh && mergedMesh->SubMeshCount() == 1u)
+    std::unique_ptr<common::Mesh> meshToDecompose =
+        std::make_unique<common::Mesh>();
+    // check if a particular submesh is requested
+    if (!_meshSdf.Submesh().empty())
+    {
+      for (unsigned int submeshIdx = 0;
+           submeshIdx < _mesh.SubMeshCount();
+           ++submeshIdx)
+      {
+        auto submesh = _mesh.SubMeshByIndex(submeshIdx).lock();
+        if (submesh->Name() == _meshSdf.Submesh())
+        {
+          if (_meshSdf.CenterSubmesh())
+            submesh->Center(math::Vector3d::Zero);
+          meshToDecompose->AddSubMesh(*submesh.get());
+          break;
+        }
+      }
+    }
+    else
+    {
+      // Merge meshes before convex decomposition
+      meshToDecompose =
+           gz::common::MeshManager::MergeSubMeshes(_mesh);
+    }
+
+    if (meshToDecompose && meshToDecompose->SubMeshCount() == 1u)
     {
       // Decompose and add mesh to MeshManager
-      auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock();
+      auto mergedSubmesh = meshToDecompose->SubMeshByIndex(0u).lock();
       std::vector<common::SubMesh> decomposed =
         gz::common::MeshManager::ConvexDecomposition(
-        *mergedSubmesh.get(), maxConvexHulls);
+        *mergedSubmesh.get(), maxConvexHulls, voxelResolution);
       gzdbg << "Optimizing mesh (" << _meshSdf.OptimizationStr() << "): "
             <<  _mesh.Name() << std::endl;
       // Create decomposed mesh and add it to MeshManager

diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc
@@ -21,6 +21,7 @@
 #include <gz/msgs/stringmsg.pb.h>
 
 #include <algorithm>
+#include <cstddef>
 #include <iostream>
 #include <map>
 #include <set>
@@ -39,6 +40,7 @@
 #include <gz/common/MeshManager.hh>
 #include <gz/common/Profiler.hh>
 #include <gz/common/StringUtils.hh>
+#include <gz/common/SubMesh.hh>
 #include <gz/common/Uuid.hh>
 
 #include <gz/gui/Application.hh>
@@ -1247,14 +1249,25 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry(
 
     // Assume absolute path to mesh file
     descriptor.meshName = fullPath;
-    descriptor.subMeshName = _geom.MeshShape()->Submesh();
-    descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh();
-
     gz::common::MeshManager *meshManager =
         gz::common::MeshManager::Instance();
     descriptor.mesh = meshManager->Load(descriptor.meshName);
     if (descriptor.mesh)
     {
+      if (_geom.MeshShape()->Optimization() != sdf::MeshOptimization::NONE)
+      {
+        const common::Mesh *optimizedMesh =
+            optimizeMesh(*_geom.MeshShape(), *descriptor.mesh);
+        if (optimizedMesh)
+        {
+          descriptor.mesh = optimizedMesh;
+          // if submesh is requested, it should be handled in the optimizeMesh
+          // function so we do not need need to pass these flags to
+          // gz-rendering
+          descriptor.subMeshName = "";
+          descriptor.centerSubMesh = false;
+        }
+      }
       geom = this->scene->CreateMesh(descriptor);
     }
     else