merge from gz-sim8

Signed-off-by: Ian Chen <ichen@openrobotics.org>

Author: iche033

CMakeLists.txt

@@ -65,6 +65,9 @@ cmake_dependent_option(USE_DIST_PACKAGES_FOR_PYTHON
 # Search for project-specific dependencies
 #============================================================================
 
+# Setting this policy enables using the protobuf_MODULE_COMPATIBLE
+# set command when cmake_minimum_required is less than 3.13
+set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
 # This option is needed to use the PROTOBUF_GENERATE_CPP
 # in case protobuf is found with the CMake config files
 # It needs to be set before any find_package(...) call

include/gz/sim/Util.hh

@@ -316,6 +316,13 @@ namespace gz
     /// \return The loaded mesh or null if the mesh can not be loaded.
     GZ_SIM_VISIBLE const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf);
 
+    /// \brief Optimize input mesh.
+    /// \param[in] _meshSdf Mesh SDF DOM with mesh optimization parameters
+    /// \param[in] _mesh Input mesh to optimize.
+    /// \return The optimized mesh or null if the mesh can not be optimized.
+    GZ_SIM_VISIBLE const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf,
+        const common::Mesh &_mesh);
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"};
 

src/Util.cc

@@ -15,12 +15,17 @@
  *
 */
 
+#include <cstddef>
+#include <string>
+#include <vector>
+
 #include <gz/msgs/entity.pb.h>
 
 #include <gz/common/Filesystem.hh>
 #include <gz/common/Mesh.hh>
 #include <gz/common/MeshManager.hh>
 #include <gz/common/StringUtils.hh>
+#include <gz/common/SubMesh.hh>
 #include <gz/common/URI.hh>
 #include <gz/common/Util.hh>
 #include <gz/math/Helpers.hh>
@@ -862,8 +867,81 @@ const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf)
            << "]." << std::endl;
     return nullptr;
   }
+
+  if (mesh && _meshSdf.Optimization() != sdf::MeshOptimization::NONE)
+  {
+    const common::Mesh *optimizedMesh = optimizeMesh(_meshSdf, *mesh);
+    if (optimizedMesh)
+      return optimizedMesh;
+    else
+      gzwarn << "Failed to optimize Mesh " << mesh->Name() << std::endl;
+  }
+
   return mesh;
 }
+
+const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf,
+    const common::Mesh &_mesh)
+{
+  if (_meshSdf.Optimization() !=
+      sdf::MeshOptimization::CONVEX_DECOMPOSITION &&
+      _meshSdf.Optimization() !=
+      sdf::MeshOptimization::CONVEX_HULL)
+    return nullptr;
+
+  auto &meshManager = *common::MeshManager::Instance();
+  std::size_t maxConvexHulls = 16u;
+  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
+    maxConvexHulls = _meshSdf.ConvexDecomposition()->MaxConvexHulls();
+  }
+  // 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);
+  auto *optimizedMesh = meshManager.MeshByName(convexMeshName);
+  if (!optimizedMesh)
+  {
+    // Merge meshes before convex decomposition
+    auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(_mesh);
+    if (mergedMesh && mergedMesh->SubMeshCount() == 1u)
+    {
+      // Decompose and add mesh to MeshManager
+      auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock();
+      std::vector<common::SubMesh> decomposed =
+        gz::common::MeshManager::ConvexDecomposition(
+        *mergedSubmesh.get(), maxConvexHulls);
+      gzdbg << "Optimizing mesh (" << _meshSdf.OptimizationStr() << "): "
+            <<  _mesh.Name() << std::endl;
+      // Create decomposed mesh and add it to MeshManager
+      // Note: MeshManager will call delete on this mesh in its destructor
+      // \todo(iche033) Consider updating MeshManager to accept
+      // unique pointers instead
+      common::Mesh *convexMesh = new common::Mesh;
+      convexMesh->SetName(convexMeshName);
+      for (const auto & submesh : decomposed)
+        convexMesh->AddSubMesh(submesh);
+      meshManager.AddMesh(convexMesh);
+      if (decomposed.empty())
+      {
+        // Print an error if convex decomposition returned empty submeshes
+        // but still add it to MeshManager to avoid going through the
+        // expensive convex decomposition process for the same mesh again
+        gzerr << "Convex decomposition generated zero meshes: "
+               << _mesh.Name() << std::endl;
+      }
+      optimizedMesh = meshManager.MeshByName(convexMeshName);
+    }
+  }
+  return optimizedMesh;
+}
+
 }
 }
 }

src/Util_TEST.cc

@@ -1023,4 +1023,12 @@ TEST_F(UtilTest, LoadMesh)
     "test", "media", "duck.dae");
   meshSdf.SetFilePath(filePath);
   EXPECT_NE(nullptr, loadMesh(meshSdf));
+
+  EXPECT_TRUE(meshSdf.SetOptimization("convex_decomposition"));
+  sdf::ConvexDecomposition convexDecomp;
+  convexDecomp.SetMaxConvexHulls(16u);
+  meshSdf.SetConvexDecomposition(convexDecomp);
+  auto *optimizedMesh = loadMesh(meshSdf);
+  EXPECT_NE(nullptr, optimizedMesh);
+  EXPECT_EQ(16u, optimizedMesh->SubMeshCount());
 }

src/rendering/RenderUtil.cc

@@ -24,6 +24,7 @@
 #include <vector>
 
 #include <sdf/Actor.hh>
+#include <sdf/Camera.hh>
 #include <sdf/Collision.hh>
 #include <sdf/Element.hh>
 #include <sdf/Joint.hh>
@@ -1323,6 +1324,18 @@ void RenderUtil::Update()
           gzerr << "Failed to create sensor [" << sensorName << "]"
                  << std::endl;
         }
+        else
+        {
+          auto sensorNode = this->dataPtr->sceneManager.NodeById(entity);
+          auto semPose = dataSdf.SemanticPose();
+          math::Pose3d sensorPose;
+          sdf::Errors errors = semPose.Resolve(sensorPose);
+          if (!errors.empty())
+          {
+            sensorPose = dataSdf.RawPose();
+          }
+          sensorNode->SetLocalPose(sensorPose);
+        }
       }
     }
   }
@@ -2388,86 +2401,6 @@ void RenderUtilPrivate::UpdateRenderingEntities(
         this->entityPoses[_entity] = _pose->Data();
         return true;
       });
-
-  // Update cameras
-  _ecm.Each<components::Camera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::Camera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update depth cameras
-  _ecm.Each<components::DepthCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::DepthCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update RGBD cameras
-  _ecm.Each<components::RgbdCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::RgbdCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update gpu_lidar
-  _ecm.Each<components::GpuLidar, components::Pose>(
-      [&](const Entity &_entity,
-        const components::GpuLidar *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update thermal cameras
-  _ecm.Each<components::ThermalCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::ThermalCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update segmentation cameras
-  _ecm.Each<components::SegmentationCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::SegmentationCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update bounding box cameras
-  _ecm.Each<components::BoundingBoxCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::BoundingBoxCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update wide angle cameras
-  _ecm.Each<components::WideAngleCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::WideAngleCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
 }
 
 //////////////////////////////////////////////////