Merge pull request #2432 from gazebosim/merge_8_main_20240605

Merge 8 -> main

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;
- });
 }
 
 //////////////////////////////////////////////////