From 50a99e6c2c102c54471e6ba364ba73d73c428696 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 21 Jul 2023 10:56:05 -0700 Subject: [PATCH] Support loading mesh by mesh name in (#2007) Allow users to specify name of mesh in SDF via . Addresses this use case: User has created common:Mesh object and added it to the MeshManager via AddMesh call. They now want to spawn a new model with this mesh using an SDF string that has set to the name of their common::Mesh. --------- Signed-off-by: Ian Chen --- include/gz/sim/Util.hh | 7 + src/Util.cc | 41 +++ src/Util_TEST.cc | 21 ++ src/rendering/SceneManager.cc | 25 +- src/systems/physics/Physics.cc | 10 +- test/integration/CMakeLists.txt | 1 + test/integration/mesh_uri.cc | 240 ++++++++++++++++++ .../worlds/camera_sensor_scene_background.sdf | 4 + tutorials/resources.md | 8 + 9 files changed, 336 insertions(+), 21 deletions(-) create mode 100644 test/integration/mesh_uri.cc diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index 41a7ca3dab..0840c2c1c1 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -24,7 +24,9 @@ #include #include +#include #include +#include #include "gz/sim/components/Environment.hh" #include "gz/sim/config.hh" @@ -309,6 +311,11 @@ namespace gz const math::Vector3d& _worldPosition, const std::shared_ptr& _gridField); + /// \brief Load a mesh from a Mesh SDF DOM + /// \param[in] _meshSdf Mesh SDF DOM + /// \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 Environment variable holding resource paths. const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"}; diff --git a/src/Util.cc b/src/Util.cc index db9918272f..e23013c241 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -18,7 +18,10 @@ #include #include +#include +#include #include +#include #include #include #include @@ -837,6 +840,44 @@ std::string resolveSdfWorldFile(const std::string &_sdfFile, return filePath; } + +const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf) +{ + const common::Mesh *mesh = nullptr; + auto &meshManager = *common::MeshManager::Instance(); + if (common::URI(_meshSdf.Uri()).Scheme() == "name") + { + // if it has a name:// scheme, see if the mesh + // exists in the mesh manager and load it by name + const std::string basename = common::basename(_meshSdf.Uri()); + mesh = meshManager.MeshByName(basename); + if (nullptr == mesh) + { + gzwarn << "Failed to load mesh by name [" << basename + << "]." << std::endl; + return nullptr; + } + } + else if (meshManager.IsValidFilename(_meshSdf.Uri())) + { + // load mesh by file path + auto fullPath = asFullPath(_meshSdf.Uri(), _meshSdf.FilePath()); + mesh = meshManager.Load(fullPath); + if (nullptr == mesh) + { + gzwarn << "Failed to load mesh from [" << fullPath + << "]." << std::endl; + return nullptr; + } + } + else + { + gzwarn << "Failed to load mesh [" << _meshSdf.Uri() + << "]." << std::endl; + return nullptr; + } + return mesh; +} } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index d4bd50edf8..6cc56dbd1b 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -40,6 +40,7 @@ #include "gz/sim/Util.hh" #include "helpers/EnvTestFixture.hh" +#include "test_config.hh" using namespace gz; using namespace sim; @@ -1003,3 +1004,23 @@ TEST_F(UtilTest, ResolveSdfWorldFile) // A bad relative path should return an empty string EXPECT_TRUE(resolveSdfWorldFile("../invalid/does_not_exist.sdf").empty()); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, LoadMesh) +{ + sdf::Mesh meshSdf; + EXPECT_EQ(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("invalid_uri"); + meshSdf.SetFilePath("invalid_filepath"); + EXPECT_EQ(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("name://unit_box"); + EXPECT_NE(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("duck.dae"); + std::string filePath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "media", "duck.dae"); + meshSdf.SetFilePath(filePath); + EXPECT_NE(nullptr, loadMesh(meshSdf)); +} diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index f1f758e8e9..5684d4b047 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -702,23 +702,22 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, } else if (_geom.Type() == sdf::GeometryType::MESH) { - auto fullPath = asFullPath(_geom.MeshShape()->Uri(), - _geom.MeshShape()->FilePath()); - if (fullPath.empty()) - { - gzerr << "Mesh geometry missing uri" << std::endl; - return geom; - } rendering::MeshDescriptor descriptor; - - // Assume absolute path to mesh file - descriptor.meshName = fullPath; + descriptor.mesh = loadMesh(*_geom.MeshShape()); + if (!descriptor.mesh) + return geom; + std::string meshUri = + (common::URI(_geom.MeshShape()->Uri()).Scheme() == "name") ? + common::basename(_geom.MeshShape()->Uri()) : + asFullPath(_geom.MeshShape()->Uri(), + _geom.MeshShape()->FilePath()); + auto a = asFullPath(_geom.MeshShape()->Uri(), + _geom.MeshShape()->FilePath()); + + descriptor.meshName = meshUri; descriptor.subMeshName = _geom.MeshShape()->Submesh(); descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh(); - common::MeshManager *meshManager = - common::MeshManager::Instance(); - descriptor.mesh = meshManager->Load(descriptor.meshName); geom = this->dataPtr->scene->CreateMesh(descriptor); scale = _geom.MeshShape()->Scale(); } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index c70e2103db..b81f17330a 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1406,15 +1406,9 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm, return true; } - auto &meshManager = *common::MeshManager::Instance(); - auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath()); - auto *mesh = meshManager.Load(fullPath); - if (nullptr == mesh) - { - gzwarn << "Failed to load mesh from [" << fullPath - << "]." << std::endl; + const common::Mesh *mesh = loadMesh(*meshSdf); + if (!mesh) return true; - } auto linkMeshFeature = this->entityLinkMap.EntityCast(_parent->Data()); diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index d702cd4012..59eae5c5c4 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -101,6 +101,7 @@ set(tests_needing_display depth_camera.cc distortion_camera.cc gpu_lidar.cc + mesh_uri.cc optical_tactile_plugin.cc reset_sensors.cc rgbd_camera.cc diff --git a/test/integration/mesh_uri.cc b/test/integration/mesh_uri.cc new file mode 100644 index 0000000000..8ce6891190 --- /dev/null +++ b/test/integration/mesh_uri.cc @@ -0,0 +1,240 @@ +/* + * Copyright (C) 2023 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 + +#include + +#include +#include +#include +#include +#include + +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "test_config.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +std::mutex mutex; +int g_count = 0; +msgs::Image g_image; + +///////////////////////////////////////////////// +void cameraCb(const msgs::Image & _msg) +{ + ASSERT_EQ(msgs::PixelFormatType::RGB_INT8, + _msg.pixel_format_type()); + + std::lock_guard lock(mutex); + g_image = _msg; + g_count++; +} + +std::string meshModelStr(bool _static = false) +{ + // SDF string consisting of a box mesh. Note that "unit_box" + // is a mesh that comes pre-registered with MeshManager so + // it should be able to load this as a mesh. + // Similarly the user can create a common::Mesh object and add that + // to the MeshManager by calling MeshManager::Instance()->AddMesh; + return std::string("") + + "" + + "" + + "" + + "" + + "name://unit_box" + + "" + + "" + + "name://unit_box" + + "" + + "" + + "" + std::to_string(_static) + "" + + "" + + ""; +} + +////////////////////////////////////////////////// +class MeshUriTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +// Test spawning mesh by name and verify that the mesh is correctly +// spawned in both rendering and physics. Cameras should see the spawned +// mesh and spawend object should collide with one another on the physics +// side. +TEST_F(MeshUriTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SpawnMeshByName)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "camera_sensor_scene_background.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + auto entityCount = ecm->EntityCount(); + + // Subscribe to the camera topic + transport::Node node; + g_count = 0; + node.Subscribe("/camera", &cameraCb); + + // Run and make sure we received camera images + server.Run(true, 100, false); + for (int i = 0; i < 100 && g_count <= 0; ++i) + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_LT(0u, g_count); + + unsigned char *imageBuffer = nullptr; + unsigned int midIdx = 0u; + unsigned int bufferSize = 0u; + // Empty scene - camera should see just red background + { + std::lock_guard lock(mutex); + bufferSize = g_image.width() *g_image.height() * 3u; + imageBuffer = new unsigned char[bufferSize]; + memcpy(imageBuffer, g_image.data().c_str(), bufferSize); + + midIdx = (g_image.height() * 0.5 * g_image.width() + + g_image.width() * 0.5) * 3; + } + unsigned int r = imageBuffer[midIdx]; + unsigned int g = imageBuffer[midIdx + 1]; + unsigned int b = imageBuffer[midIdx + 2]; + EXPECT_EQ(255u, r); + EXPECT_EQ(0u, g); + EXPECT_EQ(0u, b); + + // Spawn a static box (mesh) model in front of the camera + msgs::EntityFactory req; + req.set_sdf(meshModelStr(true)); + + auto pose = req.mutable_pose(); + auto pos = pose->mutable_position(); + pos->set_x(3); + pos->set_z(1); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/sensors/create"}; + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Check entity has not been created yet + EXPECT_EQ(kNullEntity, ecm->EntityByComponents(components::Model(), + components::Name("spawned_model"))); + + // Run an iteration and check it was created + server.Run(true, 1, false); + EXPECT_EQ(entityCount + 4, ecm->EntityCount()); + entityCount = ecm->EntityCount(); + + auto model = ecm->EntityByComponents(components::Model(), + components::Name("spawned_model")); + EXPECT_NE(kNullEntity, model); + auto poseComp = ecm->Component(model); + EXPECT_NE(nullptr, poseComp); + EXPECT_EQ(math::Pose3d(3, 0, 1, 0, 0, 0), poseComp->Data()); + + // Verify camera now sees a white box + g_count = 0u; + server.Run(true, 100, false); + for (int i = 0; i < 100 && g_count <= 1; ++i) + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_LT(0u, g_count); + + { + std::lock_guard lock(mutex); + memcpy(imageBuffer, g_image.data().c_str(), bufferSize); + } + r = imageBuffer[midIdx]; + g = imageBuffer[midIdx + 1]; + b = imageBuffer[midIdx + 2]; + EXPECT_EQ(255u, r); + EXPECT_EQ(255u, g); + EXPECT_EQ(255u, b); + + // Spawn another box over the static one + const std::string spawnedModel2Name = "spawned_model2"; + req.set_sdf(meshModelStr()); + req.set_name(spawnedModel2Name); + pose = req.mutable_pose(); + pos = pose->mutable_position(); + pos->set_x(3); + pos->set_z(5); + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Check entity has not been created yet + EXPECT_EQ(kNullEntity, ecm->EntityByComponents(components::Model(), + components::Name(spawnedModel2Name))); + + // Run an iteration and check it was created + server.Run(true, 1, false); + EXPECT_EQ(entityCount + 4, ecm->EntityCount()); + entityCount = ecm->EntityCount(); + + auto model2 = ecm->EntityByComponents(components::Model(), + components::Name(spawnedModel2Name)); + EXPECT_NE(kNullEntity, model2); + + poseComp = ecm->Component(model2); + EXPECT_NE(nullptr, poseComp); + EXPECT_EQ(math::Pose3d(3, 0, 5, 0, 0, 0), poseComp->Data()); + + // run and spawned model 2 should fall onto the previous box + server.Run(true, 1000, false); + poseComp = ecm->Component(model2); + EXPECT_NE(nullptr, poseComp); + EXPECT_EQ(math::Pose3d(3, 0, 2, 0, 0, 0), poseComp->Data()) << + poseComp->Data(); + + delete [] imageBuffer; + imageBuffer = nullptr; +} diff --git a/test/worlds/camera_sensor_scene_background.sdf b/test/worlds/camera_sensor_scene_background.sdf index b6357d6dec..f973da51fb 100644 --- a/test/worlds/camera_sensor_scene_background.sdf +++ b/test/worlds/camera_sensor_scene_background.sdf @@ -14,6 +14,10 @@ name="gz::sim::systems::Sensors"> ogre2 + + 1 0 0 diff --git a/tutorials/resources.md b/tutorials/resources.md index 09634ed9b7..02fdb5ad19 100644 --- a/tutorials/resources.md +++ b/tutorials/resources.md @@ -144,6 +144,14 @@ Gazebo will look for URIs (path / URL) in the following, in order: \* The `GZ_FILE_PATH` environment variable also works in some scenarios, but it's not recommended when using Gazebo. +If a ` starts with the `name://` scheme, +e.g. `name://my_mesh_name`, Gazebo will check to see if a mesh with the +specified name exists in the Mesh Manager and load that mesh if it exists. +This can happen when a `common::Mesh` object is created in memory and +registered with the Mesh Manager via the +[common::MeshManager::Instance()->AddMesh](https://gazebosim.org/api/common/5/classgz_1_1common_1_1MeshManager.html#a2eaddabc3a3109bd8757b2a8b2dd2d01) +call. + ### GUI configuration Gazebo Sim's