Scene update resource finder

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<msgs::StringMsg_V>(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<std::string> 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)
 {

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 rsource 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 resovled 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.

src/Server_TEST.cc

@@ -1033,6 +1033,79 @@ TEST_P(ServerFixture, AddResourcePaths)
   }
 }
 
+/////////////////////////////////////////////////
+TEST_P(ServerFixture, ResolveResourcePaths)
+{
+  ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH",
+      "/tmp/some/path:/home/user/another_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<void(const std::string _uri,
+      const std::string &_expected, bool _found)>(
+        [&](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));

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 <sdf/Imu.hh>
 #include <sdf/Lidar.hh>
 #include <sdf/Noise.hh>
+#include <sdf/Scene.hh>
 #include <sdf/Sensor.hh>
 
 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<std::string> 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 <scene> from SDF
+  // Store the Scene component data, which holds sdf::Scene so that we can
+  // populate the scene info messages.
+  auto sceneComp =
+    _manager.Component<components::Scene>(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<msgs::Scene>(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<msgs::Scene>(this->sdfScene));
 
     AddModels(&sceneMsg, this->worldEntity, newGraph);
 

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) +
+      "/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));

test/worlds/conveyor.sdf

@@ -24,6 +24,8 @@
         <scene>
             <ambient>1.0 1.0 1.0</ambient>
             <background>0.8 0.8 0.8</background>
+            <shadows>true</shadows>
+            <grid>false</grid>
         </scene>
 
         <light type="directional" name="sun">