diff --git a/CMakeLists.txt b/CMakeLists.txt index 62f2af55b5..9f309499c8 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index e8e4bed0e0..de0585e5c0 100644 --- a/include/gz/sim/Util.hh +++ b/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"}; diff --git a/src/Util.cc b/src/Util.cc index 8adfedbee7..e9cb2406f0 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -15,12 +15,17 @@ * */ +#include +#include +#include + #include #include #include #include #include +#include #include #include #include @@ -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 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; +} + } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 6be8d27903..9ec0e9be1a 100644 --- a/src/Util_TEST.cc +++ b/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()); } diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 0c833a60ca..9b2cbf8acd 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -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( - [&](const Entity &_entity, - const components::Camera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update depth cameras - _ecm.Each( - [&](const Entity &_entity, - const components::DepthCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update RGBD cameras - _ecm.Each( - [&](const Entity &_entity, - const components::RgbdCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update gpu_lidar - _ecm.Each( - [&](const Entity &_entity, - const components::GpuLidar *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update thermal cameras - _ecm.Each( - [&](const Entity &_entity, - const components::ThermalCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update segmentation cameras - _ecm.Each( - [&](const Entity &_entity, - const components::SegmentationCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update bounding box cameras - _ecm.Each( - [&](const Entity &_entity, - const components::BoundingBoxCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update wide angle cameras - _ecm.Each( - [&](const Entity &_entity, - const components::WideAngleCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); } //////////////////////////////////////////////////