diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index cfa49e98d3..b996e2fe03 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -155,6 +155,25 @@ namespace ignition std::string IGNITION_GAZEBO_VISIBLE validTopic( const std::vector &_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"}; + } } } diff --git a/src/Util.cc b/src/Util.cc index b5f9c458a9..96ad5f35dd 100644 --- a/src/Util.cc +++ b/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(_entity) ? "" : + topic = removeParentScope(removeParentScope(topic, "/"), "/"); + } + + return transport::TopicUtils::AsValidTopic("/" + topic); +} + ////////////////////////////////////////////////// std::string validTopic(const std::vector &_topics) { diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 60c0dfd909..15e12002c9 100644 --- a/src/Util_TEST.cc +++ b/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)); +} diff --git a/src/systems/particle_emitter2/ParticleEmitter2.cc b/src/systems/particle_emitter2/ParticleEmitter2.cc index 0ee8421cbf..84fe2ec90e 100644 --- a/src/systems/particle_emitter2/ParticleEmitter2.cc +++ b/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 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(