diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 7bd36065ce..55f8784154 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -356,6 +356,21 @@ void ServerPrivate::SetupTransport() << "]" << std::endl; } + // Advertise a service that returns the full path, on the Gazebo server's + // host machine, based on a provided URI. + std::string resolvePathService{"/gazebo/resource_paths/resolve"}; + if (this->node.Advertise(resolvePathService, + &ServerPrivate::ResourcePathsResolveService, this)) + { + ignmsg << "Resource path resolve service on [" << resolvePathService << "]." + << std::endl; + } + else + { + ignerr << "Something went wrong, failed to advertise [" << getPathService + << "]" << std::endl; + } + std::string pathTopic{"/gazebo/resource_paths"}; this->pathPub = this->node.Advertise(pathTopic); @@ -485,6 +500,64 @@ bool ServerPrivate::ResourcePathsService( return true; } +////////////////////////////////////////////////// +bool ServerPrivate::ResourcePathsResolveService( + const ignition::msgs::StringMsg &_req, + ignition::msgs::StringMsg &_res) +{ + // Get the request + std::string req = _req.data(); + + // Handle the case where the request is already a valid path + if (common::exists(req)) + { + _res.set_data(req); + return true; + } + + // Try Fuel + std::string path = + fuel_tools::fetchResourceWithClient(req, *this->fuelClient.get()); + if (!path.empty() && common::exists(path)) + { + _res.set_data(path); + return true; + } + + // Check for the file:// prefix. + std::string prefix = "file://"; + if (req.find(prefix) == 0) + { + req = req.substr(prefix.size()); + // Check to see if the path exists + if (common::exists(req)) + { + _res.set_data(req); + return true; + } + } + + // Check for the model:// prefix + prefix = "model://"; + if (req.find(prefix) == 0) + req = req.substr(prefix.size()); + + // Checkout resource paths + std::vector gzPaths = resourcePaths(); + for (const std::string &gzPath : gzPaths) + { + std::string fullPath = common::joinPaths(gzPath, req); + if (common::exists(fullPath)) + { + _res.set_data(fullPath); + return true; + } + } + + // Otherwise the resource could not be found + return false; +} + ////////////////////////////////////////////////// std::string ServerPrivate::FetchResource(const std::string &_uri) { diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index b7f23c26d9..db13460131 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -117,6 +117,28 @@ namespace ignition /// \return True if successful. private: bool ResourcePathsService(ignition::msgs::StringMsg_V &_res); + /// \brief Callback for a resource path resolve service. This service + /// will return the full path to a provided resource's URI. An empty + /// string and return value of false will be used if the resource could + /// not be found. + /// + /// Fuel will be checked and then the GZ_GAZEBO_RESOURCE_PATH environment + /// variable paths. This service will not check for files relative to + /// working directory of the Gazebo server. + /// + /// \param[in] _req Request filled with a resource URI to resolve. + /// Example values could be: + /// * https://URI_TO_A_FUEL_RESOURCE + /// * model://MODLE_NAME/meshes/MESH_NAME + /// * file://PATH/TO/FILE + /// + /// \param[out] _res Response filled with the resolved path, or empty + /// if the resource could not be found. + /// \return True if successful, false otherwise. + private: bool ResourcePathsResolveService( + const ignition::msgs::StringMsg &_req, + ignition::msgs::StringMsg &_res); + /// \brief Callback for server control service. /// \param[out] _req The control request. /// \param[out] _res Whether the request was successfully fullfilled. diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index a2c6d1803d..619e08b2b1 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -1033,6 +1033,78 @@ TEST_P(ServerFixture, AddResourcePaths) } } +///////////////////////////////////////////////// +TEST_P(ServerFixture, ResolveResourcePaths) +{ + ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", ""); + ignition::common::setenv("SDF_PATH", ""); + ignition::common::setenv("IGN_FILE_PATH", ""); + + ServerConfig serverConfig; + gazebo::Server server(serverConfig); + + EXPECT_FALSE(*server.Running(0)); + + auto test = std::function( + [&](const std::string &_uri, const std::string &_expected, bool _found) + { + transport::Node node; + msgs::StringMsg req, res; + bool result{false}; + bool executed{false}; + int sleep{0}; + int maxSleep{30}; + + req.set_data(_uri); + while (!executed && sleep < maxSleep) + { + igndbg << "Requesting /gazebo/resource_paths/resolve" << std::endl; + executed = node.Request("/gazebo/resource_paths/resolve", req, 100, + res, result); + sleep++; + } + EXPECT_TRUE(executed); + EXPECT_EQ(_found, result); + EXPECT_EQ(_expected, res.data()) << "Expected[" << _expected + << "] Received[" << res.data() << "]"; + }); + + // Make sure the resource path is clear + ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", ""); + + // An absolute path should return the same absolute path + test(PROJECT_SOURCE_PATH, PROJECT_SOURCE_PATH, true); + + // An absolute path, with the file:// prefix, should return the absolute path + test(std::string("file://") + + PROJECT_SOURCE_PATH, PROJECT_SOURCE_PATH, true); + + // A non-absolute path with no RESOURCE_PATH should not find the resource + test(common::joinPaths("test", "worlds", "plugins.sdf"), "", false); + + // Try again, this time with a RESOURCE_PATH + common::setenv("IGN_GAZEBO_RESOURCE_PATH", PROJECT_SOURCE_PATH); + test(common::joinPaths("test", "worlds", "plugins.sdf"), + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "plugins.sdf"), + true); + // With the file:// prefix should also work + test(std::string("file://") + + common::joinPaths("test", "worlds", "plugins.sdf"), + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "plugins.sdf"), + true); + + // The model:// URI should not resolve + test("model://include_nested/model.sdf", "", false); + ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models")); + // The model:// URI should now resolve because the RESOURCE_PATH has been + // updated. + test("model://include_nested/model.sdf", + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models", + "include_nested", "model.sdf"), true); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(ServerRepeat, ServerFixture, ::testing::Range(1, 2)); diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index d6d16803c0..91cdf15e34 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -51,6 +51,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" +#include "ignition/gazebo/components/Scene.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/ThermalCamera.hh" @@ -63,6 +64,7 @@ #include #include #include +#include #include using namespace std::chrono_literals; @@ -236,6 +238,10 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// \brief A list of async state requests public: std::unordered_set stateRequests; + + /// \brief Store SDF scene information so that it can be inserted into + /// scene message. + public: sdf::Scene sdfScene; }; ////////////////////////////////////////////////// @@ -302,8 +308,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, if (_manager.HasNewEntities()) this->dataPtr->SceneGraphAddEntities(_manager); - // Populate pose message - // TODO(louise) Get from SDF + // Store the Scene component data, which holds sdf::Scene so that we can + // populate the scene info messages. + auto sceneComp = + _manager.Component(this->dataPtr->worldEntity); + if (sceneComp) + this->dataPtr->sdfScene = sceneComp->Data(); // Create and send pose update if transport connections exist. if (this->dataPtr->dyPosePub.HasConnections() || @@ -615,6 +625,7 @@ bool SceneBroadcasterPrivate::SceneInfoService(ignition::msgs::Scene &_res) _res.Clear(); // Populate scene message + _res.CopyFrom(convert(this->sdfScene)); // Add models AddModels(&_res, this->worldEntity, this->sceneGraph); @@ -1012,6 +1023,8 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( this->SetupTransport(this->worldName); msgs::Scene sceneMsg; + // Populate scene message + sceneMsg.CopyFrom(convert(this->sdfScene)); AddModels(&sceneMsg, this->worldEntity, newGraph); diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 83ddc6cb60..41ca64e113 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -914,6 +914,44 @@ TEST_P(SceneBroadcasterTest, server.Run(true, 1, false); } +///////////////////////////////////////////////// +TEST_P(SceneBroadcasterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SceneInfoHasSceneSdf)) +{ + // Start server + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + common::joinPaths("/", "test", "worlds", "conveyor.sdf")); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + server.Run(true, 1, false); + + // Create requester + transport::Node node; + + bool result{false}; + unsigned int timeout{5000}; + ignition::msgs::Scene res; + + EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, res, result)); + EXPECT_TRUE(result); + + ASSERT_TRUE(res.has_ambient()); + EXPECT_EQ(math::Color(1.0, 1.0, 1.0, 1.0), msgs::Convert(res.ambient())); + + ASSERT_TRUE(res.has_background()); + EXPECT_EQ(math::Color(0.8, 0.8, 0.8, 1.0), msgs::Convert(res.background())); + + EXPECT_TRUE(res.shadows()); + EXPECT_FALSE(res.grid()); + EXPECT_FALSE(res.has_fog()); + EXPECT_FALSE(res.has_sky()); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, SceneBroadcasterTest, ::testing::Range(1, 2)); diff --git a/test/worlds/conveyor.sdf b/test/worlds/conveyor.sdf index f33ba10c3f..033c32dc0a 100644 --- a/test/worlds/conveyor.sdf +++ b/test/worlds/conveyor.sdf @@ -24,6 +24,8 @@ 1.0 1.0 1.0 0.8 0.8 0.8 + true + false