Generate valid topics everywhere (support names with spaces) (#522)

Signed-off-by: Louise Poubel <louise@openrobotics.org>

CMakeLists.txt

@@ -75,7 +75,7 @@ set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools5_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-gui
-ign_find_package(ignition-gui4 REQUIRED VERSION 4.1)
+ign_find_package(ignition-gui4 REQUIRED VERSION 4.1.1)
 set(IGN_GUI_VER ${ignition-gui4_VERSION_MAJOR})
 ign_find_package (Qt5
   COMPONENTS

examples/worlds/spaces.sdf

@@ -0,0 +1,108 @@
+<?xml version="1.0" ?>
+<!--
+
+This demo shows that it's possible to have entities with spaces in their names.
+
+-->
+<sdf version="1.6">
+  <world name="world with spaces">
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <scene>
+      <ambient>1.0 1.0 1.0</ambient>
+      <background>0.8 0.8 0.8</background>
+    </scene>
+
+    <light type="directional" name="light with spaces">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.8 0.8 0.8 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="model with spaces">
+      <pose>0 0 0.5 0 0 0</pose>
+      <link name="link with spaces">
+        <inertial>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="collision with spaces">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <visual name="visual with spaces">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>1 0 0 1</diffuse>
+            <specular>1 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>

include/ignition/gazebo/Util.hh

@@ -139,6 +139,21 @@ namespace ignition
         const Entity &_entity,
         const EntityComponentManager &_ecm);
 
+    /// \brief Helper function to generate a valid transport topic, given
+    /// a list of topics ordered by preference. The generated topic will be,
+    /// in this order:
+    ///
+    /// 1. The first topic unchanged, if valid.
+    /// 2. A valid version of the first topic, if possible.
+    /// 3. The second topic unchanged, if valid.
+    /// 4. A valid version of the second topic, if possible.
+    /// 5. ...
+    /// 6. If no valid topics could be generated, return an empty string.
+    ///
+    /// \param[in] _topics Topics ordered by preference.
+    std::string IGNITION_GAZEBO_VISIBLE validTopic(
+        const std::vector<std::string> &_topics);
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"};
 

src/LevelManager.cc

@@ -76,8 +76,14 @@ LevelManager::LevelManager(SimulationRunner *_runner, const bool _useLevels)
   this->ReadLevelPerformerInfo();
   this->CreatePerformers();
 
-  std::string service = "/world/";
-  service += this->runner->sdfWorld->Name() + "/level/set_performer";
+  std::string service = transport::TopicUtils::AsValidTopic("/world/" +
+      this->runner->sdfWorld->Name() + "/level/set_performer");
+  if (service.empty())
+  {
+    ignerr << "Failed to generate set_performer topic for world ["
+           << this->runner->sdfWorld->Name() << "]" << std::endl;
+    return;
+  }
   this->node.Advertise(service, &LevelManager::OnSetPerformer, this);
 }
 

src/SimulationRunner.cc

@@ -157,15 +157,20 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
 
   // World control
   transport::NodeOptions opts;
+  std::string ns{"/world/" + this->worldName};
   if (this->networkMgr)
   {
-    opts.SetNameSpace(this->networkMgr->Namespace() +
-                      "/world/" + this->worldName);
+    ns = this->networkMgr->Namespace() + ns;
   }
-  else
+
+  auto validNs = transport::TopicUtils::AsValidTopic(ns);
+  if (validNs.empty())
   {
-    opts.SetNameSpace("/world/" + this->worldName);
+    ignerr << "Invalid namespace [" << ns
+           << "], not initializing runner transport." << std::endl;
+    return;
   }
+  opts.SetNameSpace(validNs);
 
   this->node = std::make_unique<transport::Node>(opts);
 

src/Util.cc

@@ -24,6 +24,7 @@
 #endif
 #include <ignition/common/Filesystem.hh>
 #include <ignition/common/StringUtils.hh>
+#include <ignition/transport/TopicUtils.hh>
 
 #include "ignition/gazebo/components/Actor.hh"
 #include "ignition/gazebo/components/Collision.hh"
@@ -416,6 +417,27 @@ ignition::gazebo::Entity topLevelModel(const Entity &_entity,
   }
   return entity;
 }
+
+//////////////////////////////////////////////////
+std::string validTopic(const std::vector<std::string> &_topics)
+{
+  for (const auto &topic : _topics)
+  {
+    auto validTopic = transport::TopicUtils::AsValidTopic(topic);
+    if (validTopic.empty())
+    {
+      ignerr << "Topic [" << topic << "] is invalid, ignoring." << std::endl;
+      continue;
+    }
+    if (validTopic != topic)
+    {
+      igndbg << "Topic [" << topic << "] changed to valid topic ["
+             << validTopic << "]" << std::endl;
+    }
+    return validTopic;
+  }
+  return std::string();
+}
 }
 }
 }

src/Util_TEST.cc

@@ -16,6 +16,7 @@
 */
 
 #include <gtest/gtest.h>
+#include <ignition/common/Console.hh>
 #include <sdf/Actor.hh>
 #include <sdf/Light.hh>
 
@@ -36,8 +37,18 @@
 using namespace ignition;
 using namespace gazebo;
 
+/// \brief Tests for Util.hh
+class UtilTest : public ::testing::Test
+{
+  // Documentation inherited
+  protected: void SetUp() override
+  {
+    common::Console::SetVerbosity(4);
+  }
+};
+
 /////////////////////////////////////////////////
-TEST(UtilTest, ScopedName)
+TEST_F(UtilTest, ScopedName)
 {
   EntityComponentManager ecm;
 
@@ -219,7 +230,7 @@ TEST(UtilTest, ScopedName)
 }
 
 /////////////////////////////////////////////////
-TEST(UtilTest, EntityTypeId)
+TEST_F(UtilTest, EntityTypeId)
 {
   EntityComponentManager ecm;
 
@@ -264,7 +275,7 @@ TEST(UtilTest, EntityTypeId)
 }
 
 /////////////////////////////////////////////////
-TEST(UtilTest, EntityTypeStr)
+TEST_F(UtilTest, EntityTypeStr)
 {
   EntityComponentManager ecm;
 
@@ -309,7 +320,7 @@ TEST(UtilTest, EntityTypeStr)
 }
 
 /////////////////////////////////////////////////
-TEST(UtilTest, RemoveParentScopedName)
+TEST_F(UtilTest, RemoveParentScopedName)
 {
   EXPECT_EQ(removeParentScope("world/world_name", "/"), "world_name");
   EXPECT_EQ(removeParentScope("world::world_name::light::lightA_name", "::"),
@@ -324,7 +335,7 @@ TEST(UtilTest, RemoveParentScopedName)
 }
 
 /////////////////////////////////////////////////
-TEST(UtilTest, AsFullPath)
+TEST_F(UtilTest, AsFullPath)
 {
   const std::string relativeUriUnix{"meshes/collision.dae"};
   const std::string relativeUriWindows{"meshes\\collision.dae"};
@@ -408,7 +419,7 @@ TEST(UtilTest, AsFullPath)
 }
 
 /////////////////////////////////////////////////
-TEST(UtilTest, TopLevelModel)
+TEST_F(UtilTest, TopLevelModel)
 {
   EntityComponentManager ecm;
 
@@ -463,3 +474,27 @@ TEST(UtilTest, TopLevelModel)
   // model C should have itself as the top level entity
   EXPECT_EQ(modelCEntity, topLevelModel(modelCEntity, ecm));
 }
+
+/////////////////////////////////////////////////
+TEST_F(UtilTest, ValidTopic)
+{
+  std::string good{"good"};
+  std::string fixable{"not bad~"};
+  std::string invalid{"@~@~@~"};
+
+  EXPECT_EQ("good", validTopic({good}));
+  EXPECT_EQ("not_bad", validTopic({fixable}));
+  EXPECT_EQ("", validTopic({invalid}));
+
+  EXPECT_EQ("good", validTopic({good, fixable}));
+  EXPECT_EQ("not_bad", validTopic({fixable, good}));
+
+  EXPECT_EQ("good", validTopic({good, invalid}));
+  EXPECT_EQ("good", validTopic({invalid, good}));
+
+  EXPECT_EQ("not_bad", validTopic({fixable, invalid}));
+  EXPECT_EQ("not_bad", validTopic({invalid, fixable}));
+
+  EXPECT_EQ("not_bad", validTopic({fixable, invalid, good}));
+  EXPECT_EQ("good", validTopic({invalid, good, fixable}));
+}

src/gui/Gui.cc

@@ -194,18 +194,31 @@ std::unique_ptr<ignition::gui::Application> createGui(
 
       // Request GUI info for each world
       result = false;
-      service = std::string("/world/" + worldName + "/gui/info");
-
-      igndbg << "Requesting GUI from [" << service << "]..." << std::endl;
-
-      // Request and block
       ignition::msgs::GUI res;
-      executed = node.Request(service, timeout, res, result);
+      service = transport::TopicUtils::AsValidTopic("/world/" + worldName +
+          "/gui/info");
+      if (service.empty())
+      {
+        ignerr << "Failed to generate valid service for world [" << worldName
+               << "]" << std::endl;
+      }
+      else
+      {
+        igndbg << "Requesting GUI from [" << service << "]..." << std::endl;
 
-      if (!executed)
-        ignerr << "Service call timed out for [" << service << "]" << std::endl;
-      else if (!result)
-        ignerr << "Service call failed for [" << service << "]" << std::endl;
+        // Request and block
+        executed = node.Request(service, timeout, res, result);
+
+        if (!executed)
+        {
+          ignerr << "Service call timed out for [" << service << "]"
+                 << std::endl;
+        }
+        else if (!result)
+        {
+          ignerr << "Service call failed for [" << service << "]" << std::endl;
+        }
+      }
 
       // GUI runner
       auto runner = new ignition::gazebo::GuiRunner(worldName);

src/gui/GuiRunner.cc

@@ -40,7 +40,14 @@ GuiRunner::GuiRunner(const std::string &_worldName)
   winWorldNames.append(QString::fromStdString(_worldName));
   win->setProperty("worldNames", winWorldNames);
 
-  this->stateTopic = "/world/" + _worldName + "/state";
+  this->stateTopic = transport::TopicUtils::AsValidTopic("/world/" +
+      _worldName + "/state");
+  if (this->stateTopic.empty())
+  {
+    ignerr << "Failed to generate valid topic for world [" << _worldName << "]"
+           << std::endl;
+    return;
+  }
 
   common::addFindFileURICallback([] (common::URI _uri)
   {
@@ -63,6 +70,15 @@ void GuiRunner::RequestState()
   std::string id = std::to_string(gui::App()->applicationPid());
   std::string reqSrv =
       this->node.Options().NameSpace() + "/" + id + "/state_async";
+  auto reqSrvValid = transport::TopicUtils::AsValidTopic(reqSrv);
+  if (reqSrvValid.empty())
+  {
+    ignerr << "Failed to generate valid service [" << reqSrv << "]"
+           << std::endl;
+    return;
+  }
+  reqSrv = reqSrvValid;
+
   this->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this);
   ignition::msgs::StringMsg req;
   req.set_data(reqSrv);