Resource env var, with transport interface (#172)

Signed-off-by: Louise Poubel <[email protected]>

Author: chapulina

include/ignition/gazebo/Server.hh

@@ -82,6 +82,12 @@ namespace ignition
     ///   1. /world/<world_name>/scene/info(none) : ignition::msgs::Scene
     ///     + Returns the current scene information.
     ///
+    ///   2. /gazebo/resource_paths/get : ignition::msgs::StringMsg_V
+    ///     + Get list of resource paths.
+    ///
+    ///   3. /gazebo/resource_paths/add : ignition::msgs::Empty
+    ///     + Add new resource paths.
+    ///
     /// ## Topics
     ///
     /// The following are topics provided by the Server.
@@ -95,6 +101,9 @@ namespace ignition
     /// 2. /world/<world_name>/stats : ignition::msgs::WorldStatistics
     ///   + This topic is throttled to 5Hz.
     ///
+    /// 3. /gazebo/resource_paths : ignition::msgs::StringMsg_V
+    ///   + Updated list of resource paths.
+    ///
     class IGNITION_GAZEBO_VISIBLE Server
     {
       /// \brief Construct the server using the parameters specified in a

include/ignition/gazebo/Util.hh

@@ -18,6 +18,7 @@
 #define IGNITION_GAZEBO_UTIL_HH_
 
 #include <string>
+#include <vector>
 
 #include <ignition/math/Pose3.hh>
 #include "ignition/gazebo/config.hh"
@@ -66,8 +67,26 @@ namespace ignition
     /// paths.
     /// \param[in] _filePath The path to a file in disk.
     /// \return The full path URI.
-    std::string asFullPath(const std::string &_uri,
+    std::string IGNITION_GAZEBO_VISIBLE asFullPath(const std::string &_uri,
         const std::string &_filePath);
+
+    /// \brief Get resource paths based on latest environment variables.
+    /// \return All paths in the IGN_GAZEBO_RESOURCE_PATH variable.
+    std::vector<std::string> IGNITION_GAZEBO_VISIBLE resourcePaths();
+
+    /// \brief Add resource paths based on latest environment variables.
+    /// This will update the SDF and Ignition environment variables, and
+    /// optionally add more paths to the list.
+    /// \param[in] _paths Optional paths to add.
+    void IGNITION_GAZEBO_VISIBLE addResourcePaths(
+        const std::vector<std::string> &_paths = {});
+
+    /// \brief Environment variable holding resource paths.
+    const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"};
+
+    /// \brief Environment variable used by SDFormat to find URIs inside
+    /// `<include>`
+    const std::string kSdfPathEnv{"SDF_PATH"};
     }
   }
 }

src/SdfGenerator_TEST.cc

@@ -52,19 +52,35 @@ static bool isSubset(const sdf::ElementPtr &_elemA,
                      const sdf::ElementPtr &_elemB)
 {
   if (_elemA->GetName() != _elemB->GetName())
+  {
+    igndbg << "[" << _elemA->GetName() << "] different from ["
+            << _elemB->GetName() << "]" << std::endl;
     return false;
+  }
   if (_elemA->GetAttributeCount() != _elemB->GetAttributeCount())
+  {
+    igndbg << "[" << _elemA->GetAttributeCount() << "] different from ["
+            << _elemB->GetAttributeCount() << "]" << std::endl;
     return false;
+  }
 
   // Compare attributes
   for (std::size_t i = 0; i < _elemA->GetAttributeCount(); ++i)
   {
     sdf::ParamPtr attrA = _elemA->GetAttribute(i);
     sdf::ParamPtr attrB = _elemB->GetAttribute(attrA->GetKey());
     if (attrA->GetTypeName() != attrB->GetTypeName())
+    {
+      igndbg << "[" << attrA->GetTypeName() << "] different from ["
+              << attrB->GetTypeName() << "]" << std::endl;
       return false;
+    }
     if (attrA->GetAsString() != attrB->GetAsString())
+    {
+      igndbg << "[" << attrA->GetAsString() << "] different from ["
+              << attrB->GetAsString() << "]" << std::endl;
       return false;
+    }
   }
   // Compare values
   {

src/Server.cc

@@ -14,7 +14,6 @@
  * limitations under the License.
  *
 */
-#include "ignition/gazebo/Server.hh"
 
 #include <ignition/common/SystemPaths.hh>
 #include <ignition/fuel_tools/Interface.hh>
@@ -23,6 +22,9 @@
 #include <sdf/Error.hh>
 
 #include "ignition/gazebo/config.hh"
+#include "ignition/gazebo/Server.hh"
+#include "ignition/gazebo/Util.hh"
+
 #include "ServerPrivate.hh"
 #include "SimulationRunner.hh"
 
@@ -100,6 +102,8 @@ Server::Server(const ServerConfig &_config)
   common::addFindFileURICallback(std::bind(&ServerPrivate::FetchResourceUri,
       this->dataPtr.get(), std::placeholders::_1));
 
+  addResourcePaths();
+
   sdf::Errors errors;
 
   // Load a world if specified. Check SDF string first, then SDF file
@@ -120,7 +124,7 @@ Server::Server(const ServerConfig &_config)
   else if (!_config.SdfFile().empty())
   {
     common::SystemPaths systemPaths;
-    systemPaths.SetFilePathEnv("IGN_GAZEBO_RESOURCE_PATH");
+    systemPaths.SetFilePathEnv(kResourcePathEnv);
     systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR);
     std::string filePath = systemPaths.FindFile(_config.SdfFile());
     ignmsg << "Loading SDF world file[" << filePath << "].\n";

src/ServerPrivate.cc

@@ -22,11 +22,13 @@
 #include <sdf/World.hh>
 
 #include <ignition/common/Console.hh>
+#include <ignition/common/Util.hh>
 
 #include <ignition/fuel_tools/Interface.hh>
 
 #include <ignition/gui/Application.hh>
 
+#include "ignition/gazebo/Util.hh"
 #include "SimulationRunner.hh"
 
 using namespace ignition;
@@ -384,7 +386,26 @@ void ServerPrivate::CreateEntities()
 void ServerPrivate::SetupTransport()
 {
   // Advertise available worlds.
-  this->node.Advertise("/gazebo/worlds", &ServerPrivate::WorldsService, this);
+  std::string worldsService{"/gazebo/worlds"};
+  this->node.Advertise(worldsService, &ServerPrivate::WorldsService, this);
+
+  ignmsg << "Serving world names on [" << worldsService << "]" << std::endl;
+
+  // Resource path management
+  std::string addPathService{"/gazebo/resource_paths/add"};
+  this->node.Advertise(addPathService,
+      &ServerPrivate::AddResourcePathsService, this);
+
+  std::string getPathService{"/gazebo/resource_paths/get"};
+  this->node.Advertise(getPathService,
+      &ServerPrivate::ResourcePathsService, this);
+
+  std::string pathTopic{"/gazebo/resource_paths"};
+  this->pathPub = this->node.Advertise<msgs::StringMsg_V>(pathTopic);
+
+  ignmsg << "Resource path interfaces on [" << addPathService
+         << "], [" << getPathService << "], and [" << pathTopic << "]."
+         << std::endl;
 }
 
 //////////////////////////////////////////////////
@@ -402,6 +423,49 @@ bool ServerPrivate::WorldsService(ignition::msgs::StringMsg_V &_res)
   return true;
 }
 
+//////////////////////////////////////////////////
+void ServerPrivate::AddResourcePathsService(
+    const ignition::msgs::StringMsg_V &_req)
+{
+  std::vector<std::string> paths;
+  for (int i = 0; i < _req.data_size(); ++i)
+  {
+    paths.push_back(_req.data(i));
+  }
+  addResourcePaths(paths);
+
+  // Notify new paths
+  msgs::StringMsg_V msg;
+  auto gzPaths = resourcePaths();
+  for (const auto &path : gzPaths)
+  {
+    if (!path.empty())
+      msg.add_data(path);
+  }
+
+  this->pathPub.Publish(msg);
+}
+
+//////////////////////////////////////////////////
+bool ServerPrivate::ResourcePathsService(
+    ignition::msgs::StringMsg_V &_res)
+{
+  _res.Clear();
+
+  // Update paths
+  addResourcePaths();
+
+  // Get paths
+  auto gzPaths = resourcePaths();
+  for (const auto &path : gzPaths)
+  {
+    if (!path.empty())
+      _res.add_data(path);
+  }
+
+  return true;
+}
+
 //////////////////////////////////////////////////
 std::string ServerPrivate::FetchResource(const std::string &_uri)
 {

src/ServerPrivate.hh

@@ -103,6 +103,16 @@ namespace ignition
       /// \return True if successful.
       private: bool WorldsService(ignition::msgs::StringMsg_V &_res);
 
+      /// \brief Callback for add resource paths service.
+      /// \param[out] _req Request containing the paths to be added.
+      private: void AddResourcePathsService(
+          const ignition::msgs::StringMsg_V &_req);
+
+      /// \brief Callback for get resource paths service.
+      /// \param[out] _res Response filled with all current paths.
+      /// \return True if successful.
+      private: bool ResourcePathsService(ignition::msgs::StringMsg_V &_res);
+
       /// \brief A pool of worker threads.
       public: common::WorkerPool workerPool{2};
 
@@ -148,6 +158,9 @@ namespace ignition
 
       /// \brief Node for transport.
       private: transport::Node node;
+
+      /// \brief Publisher of resrouce paths.
+      private: transport::Node::Publisher pathPub;
     };
     }
   }