Added topic name helper function

Signed-off-by: Nate Koenig <nate@openrobotics.org>

include/ignition/gazebo/Util.hh

@@ -155,6 +155,25 @@ namespace ignition
     std::string IGNITION_GAZEBO_VISIBLE validTopic(
         const std::vector<std::string> &_topics);
 
+    /// \brief Helper function that returns a valid Ignition Transport topic
+    /// consisting of the scoped name for the provided entity.
+    ///
+    /// For example, if the provided entity has a scoped name of
+    /// `my_model::my_link::my_sensor` then the resulting topic name will
+    /// be `/model/my_model/link/my_link/sensor/my_sensor`. If _excludeWorld
+    /// is false, then the topic name will be prefixed by `/world/WORLD_NAME/`,
+    /// where `WORLD_NAME` is the name of the world.
+    ///
+    /// \param[in] _entity The entity to generate the topic name for.
+    /// \param[in] _ecm The entity component manager.
+    /// \param[in] _excludeWorld True to exclude the world name from the topic.
+    /// \return An Ignition Transport topic name based on the scoped name of
+    /// the provided entity, or empty string if a topic name could not be
+    /// generated.
+    std::string topicFromScopedName(const Entity &_entity,
+        const EntityComponentManager &_ecm,
+        bool _excludeWorld = true);
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"};
 
@@ -168,6 +187,7 @@ namespace ignition
     /// \brief Environment variable holding paths to custom rendering engine
     /// plugins.
     const std::string kRenderPluginPathEnv{"IGN_GAZEBO_RENDER_ENGINE_PATH"};
+
     }
   }
 }

src/Util.cc

@@ -444,6 +444,23 @@ ignition::gazebo::Entity topLevelModel(const Entity &_entity,
   return modelEntity;
 }
 
+//////////////////////////////////////////////////
+std::string topicFromScopedName(const Entity &_entity,
+    const EntityComponentManager &_ecm, bool _excludeWorld)
+{
+  std::string topic = scopedName(_entity, _ecm, "/", true);
+
+  if (_excludeWorld)
+  {
+    // Exclude the world name. If the entity is a world, then return an
+    // empty string.
+    topic = _ecm.Component<components::World>(_entity) ? "" :
+      topic = removeParentScope(removeParentScope(topic, "/"), "/");
+  }
+
+  return transport::TopicUtils::AsValidTopic("/" + topic);
+}
+
 //////////////////////////////////////////////////
 std::string validTopic(const std::vector<std::string> &_topics)
 {

src/Util_TEST.cc

@@ -519,3 +519,93 @@ TEST_F(UtilTest, ValidTopic)
   EXPECT_EQ("not_bad", validTopic({fixable, invalid, good}));
   EXPECT_EQ("good", validTopic({invalid, good, fixable}));
 }
+
+/////////////////////////////////////////////////
+TEST_F(UtilTest, TopicFromScopedName)
+{
+  EntityComponentManager ecm;
+
+  // world
+  //  - modelA
+  //    - linkA
+  //    - modelB
+  //      - linkB
+  //        - emitterB
+  //  - modelC
+
+  // World
+  auto worldEntity = ecm.CreateEntity();
+  ecm.CreateComponent(worldEntity, components::World());
+  ecm.CreateComponent(worldEntity, components::Name("world_name"));
+
+  // Model A
+  auto modelAEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelAEntity, components::Model());
+  ecm.CreateComponent(modelAEntity, components::Name("modelA_name"));
+  ecm.CreateComponent(modelAEntity, components::ParentEntity(worldEntity));
+
+  // Link A - Child of Model A
+  auto linkAEntity = ecm.CreateEntity();
+  ecm.CreateComponent(linkAEntity, components::Link());
+  ecm.CreateComponent(linkAEntity, components::Name("linkA_name"));
+  ecm.CreateComponent(linkAEntity, components::ParentEntity(modelAEntity));
+
+  // Model B - nested inside Model A
+  auto modelBEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelBEntity, components::Model());
+  ecm.CreateComponent(modelBEntity, components::Name("modelB_name"));
+  ecm.CreateComponent(modelBEntity, components::ParentEntity(modelAEntity));
+
+  // Link B - child of Model B
+  auto linkBEntity = ecm.CreateEntity();
+  ecm.CreateComponent(linkBEntity, components::Link());
+  ecm.CreateComponent(linkBEntity, components::Name("linkB_name"));
+  ecm.CreateComponent(linkBEntity, components::ParentEntity(modelBEntity));
+
+  // Emitter B - child of Link B
+  auto emitterBEntity = ecm.CreateEntity();
+  ecm.CreateComponent(emitterBEntity, components::ParticleEmitter());
+  ecm.CreateComponent(emitterBEntity, components::Name("emitterB_name"));
+  ecm.CreateComponent(emitterBEntity, components::ParentEntity(linkBEntity));
+
+  // Model C
+  auto modelCEntity = ecm.CreateEntity();
+  ecm.CreateComponent(modelCEntity, components::Model());
+  ecm.CreateComponent(modelCEntity, components::Name("modelC_name"));
+  ecm.CreateComponent(modelCEntity, components::ParentEntity(worldEntity));
+
+  std::string testName = "/model/modelA_name";
+  std::string worldName = "/world/world_name";
+  // model A, link A, model B, link B and visual B should have
+  // model A as the top level model
+  EXPECT_EQ(testName, topicFromScopedName(modelAEntity, ecm));
+  EXPECT_EQ(worldName + testName,
+      topicFromScopedName(modelAEntity, ecm, false));
+
+  testName += "/link/linkA_name";
+  EXPECT_EQ(testName, topicFromScopedName(linkAEntity, ecm));
+  EXPECT_EQ(worldName + testName, topicFromScopedName(linkAEntity, ecm, false));
+
+  testName = "/model/modelA_name/model/modelB_name";
+  EXPECT_EQ(testName, topicFromScopedName(modelBEntity, ecm));
+  EXPECT_EQ(worldName + testName,
+      topicFromScopedName(modelBEntity, ecm, false));
+
+  testName +="/link/linkB_name";
+  EXPECT_EQ(testName, topicFromScopedName(linkBEntity, ecm));
+  EXPECT_EQ(worldName + testName, topicFromScopedName(linkBEntity, ecm, false));
+
+  testName += "/particle_emitter/emitterB_name";
+  EXPECT_EQ(testName,
+      topicFromScopedName(emitterBEntity, ecm));
+  EXPECT_EQ(worldName + testName,
+      topicFromScopedName(emitterBEntity, ecm, false));
+
+  testName = "/model/modelC_name";
+  EXPECT_EQ(testName, topicFromScopedName(modelCEntity, ecm));
+  EXPECT_EQ(worldName + testName,
+      topicFromScopedName(modelCEntity, ecm, false));
+
+  EXPECT_TRUE(topicFromScopedName(worldEntity, ecm).empty());
+  EXPECT_EQ(worldName, topicFromScopedName(worldEntity, ecm, false));
+}

src/systems/particle_emitter2/ParticleEmitter2.cc

@@ -180,34 +180,9 @@ void ParticleEmitter2::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
         }
 
         // If a topic has not been specified, then generate topic based
-        // on the model, link, and emitter names.
-        if (topic.empty())
-        {
-          std::string emitterScopedName =
-            removeParentScope(scopedName(_entity, _ecm, "/", false), "/");
-
-          std::vector<std::string> nameParts = common::split(
-              emitterScopedName, "/");
-
-          if (nameParts.size() == 3)
-          {
-            topic = "/model/" + nameParts[0] + "/link/" + nameParts[1] +
-              "/particle_emitter/" + nameParts[2] + "/cmd";
-          }
-          // Handle nested models
-          else if (nameParts.size() == 4)
-          {
-            topic = "/model/" + nameParts[0] + "/model/" + nameParts[1] +
-              "/link/" + nameParts[2] + "/particle_emitter/" + nameParts[3] +
-              "/cmd";
-          }
-          else
-          {
-            ignerr << "Particle emitter missing model name, link name, or its "
-            "own name.\n";
-            return false;
-          }
-        }
+        // on the scoped name.
+        topic = !topic.empty() ? topic :
+          topicFromScopedName(_entity, _ecm) + "/cmd";
 
         // Subscribe to the topic that receives particle emitter commands.
         if (!this->dataPtr->node.Subscribe(