diff --git a/examples/worlds/particle_emitter.sdf b/examples/worlds/particle_emitter.sdf index 58fb36c3be..bf372aeef2 100644 --- a/examples/worlds/particle_emitter.sdf +++ b/examples/worlds/particle_emitter.sdf @@ -1,5 +1,10 @@ + + + + + + + + 0.001 + 1.0 + + + + libignition-physics-tpe-plugin.so + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/fog generator + + + + + true + 20 20 0 0 0 0 + + + + true + 10 10 0 + 1 1 1 + 25 + 0.1 + 0.2 + 0.5 + 5 + + 1.0 0.0 0.0 + 1.0 0.0 0.0 + + + + + + + + diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index a8740a291e..e880d54be9 100644 --- a/include/ignition/gazebo/Conversions.hh +++ b/include/ignition/gazebo/Conversions.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -47,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -624,6 +626,42 @@ namespace ignition /// \return Axis aligned box object. template<> math::AxisAlignedBox convert(const msgs::AxisAlignedBox &_in); + + /// \brief Generic conversion from a particle emitter SDF object to another + /// type. + /// \param[in] _in Particle emitter SDF object. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::ParticleEmitter &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a particle emitter SDF object to + /// a particle emitter message object. + /// \param[in] _in Particle emitter SDF object. + /// \return Particle emitter message. + template<> + msgs::ParticleEmitter convert(const sdf::ParticleEmitter &_in); + + /// \brief Generic conversion from a particle emitter SDF object to another + /// type. + /// \param[in] _in Particle emitter SDF object. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const msgs::ParticleEmitter &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a particle emitter SDF object to + /// a particle emitter message object. + /// \param[in] _in Particle emitter SDF object. + /// \return Particle emitter message. + template<> + sdf::ParticleEmitter convert(const msgs::ParticleEmitter &_in); } } } diff --git a/include/ignition/gazebo/SdfEntityCreator.hh b/include/ignition/gazebo/SdfEntityCreator.hh index 8ad31f32bb..94e4b9616f 100644 --- a/include/ignition/gazebo/SdfEntityCreator.hh +++ b/include/ignition/gazebo/SdfEntityCreator.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -140,6 +141,13 @@ namespace ignition /// \sa CreateEntities(const sdf::Model *) public: Entity CreateEntities(const sdf::Sensor *_sensor); + /// \brief Create all entities that exist in the + /// sdf::ParticleEmitter object. + /// \param[in] _emitter SDF ParticleEmitter object. + /// \return ParticleEmitter entity. + /// \sa CreateEntities(const sdf::Link *) + public: Entity CreateEntities(const sdf::ParticleEmitter *_emitter); + /// \brief Request an entity deletion. This will insert the request /// into a queue. The queue is processed toward the end of a simulation /// update step. 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/Conversions.cc b/src/Conversions.cc index f5389fbe07..6852fa916e 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -1326,3 +1326,130 @@ math::AxisAlignedBox ignition::gazebo::convert(const msgs::AxisAlignedBox &_in) out.Max() = msgs::Convert(_in.max_corner()); return out; } + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +msgs::ParticleEmitter ignition::gazebo::convert(const sdf::ParticleEmitter &_in) +{ + msgs::ParticleEmitter out; + out.set_name(_in.Name()); + switch(_in.Type()) + { + default: + case sdf::ParticleEmitterType::POINT: + out.set_type(msgs::ParticleEmitter::POINT); + break; + case sdf::ParticleEmitterType::BOX: + out.set_type(msgs::ParticleEmitter::BOX); + break; + case sdf::ParticleEmitterType::CYLINDER: + out.set_type(msgs::ParticleEmitter::CYLINDER); + break; + case sdf::ParticleEmitterType::ELLIPSOID: + out.set_type(msgs::ParticleEmitter::ELLIPSOID); + break; + } + + msgs::Set(out.mutable_pose(), _in.RawPose()); + msgs::Set(out.mutable_size(), _in.Size()); + msgs::Set(out.mutable_particle_size(), _in.ParticleSize()); + out.mutable_rate()->set_data(_in.Rate()); + out.mutable_duration()->set_data(_in.Duration()); + out.mutable_emitting()->set_data(_in.Emitting()); + out.mutable_lifetime()->set_data(_in.Lifetime()); + if (_in.Material()) + { + out.mutable_material()->CopyFrom(convert(*_in.Material())); + } + out.mutable_min_velocity()->set_data(_in.MinVelocity()); + out.mutable_max_velocity()->set_data(_in.MaxVelocity()); + msgs::Set(out.mutable_color_start(), _in.ColorStart()); + msgs::Set(out.mutable_color_end(), _in.ColorEnd()); + out.mutable_scale_rate()->set_data(_in.ScaleRate()); + out.mutable_color_range_image()->set_data(_in.ColorRangeImage()); + + if (!_in.ColorRangeImage().empty()) + { + std::string path = asFullPath(_in.ColorRangeImage(), _in.FilePath()); + + common::SystemPaths systemPaths; + systemPaths.SetFilePathEnv(kResourcePathEnv); + std::string absolutePath = systemPaths.FindFile(path); + + if (!absolutePath.empty()) + out.mutable_color_range_image()->set_data(absolutePath); + } + + /// \todo(nkoenig) Modify the particle_emitter.proto file to + /// have a topic field. + if (!_in.Topic().empty()) + { + auto header = out.mutable_header()->add_data(); + header->set_key("topic"); + header->add_value(_in.Topic()); + } + + return out; +} + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +sdf::ParticleEmitter ignition::gazebo::convert(const msgs::ParticleEmitter &_in) +{ + sdf::ParticleEmitter out; + out.SetName(_in.name()); + switch(_in.type()) + { + default: + case msgs::ParticleEmitter::POINT: + out.SetType(sdf::ParticleEmitterType::POINT); + break; + case msgs::ParticleEmitter::BOX: + out.SetType(sdf::ParticleEmitterType::BOX); + break; + case msgs::ParticleEmitter::CYLINDER: + out.SetType(sdf::ParticleEmitterType::CYLINDER); + break; + case msgs::ParticleEmitter::ELLIPSOID: + out.SetType(sdf::ParticleEmitterType::ELLIPSOID); + break; + } + out.SetRawPose(msgs::Convert(_in.pose())); + out.SetSize(msgs::Convert(_in.size())); + out.SetParticleSize(msgs::Convert(_in.particle_size())); + out.SetMinVelocity(msgs::Convert(_in.min_velocity())); + out.SetMaxVelocity(msgs::Convert(_in.max_velocity())); + out.SetColorStart(msgs::Convert(_in.color_start())); + out.SetColorEnd(msgs::Convert(_in.color_end())); + + if (_in.has_material()) + { + out.SetMaterial(convert(_in.material())); + } + + if (_in.has_rate()) + out.SetRate(_in.rate().data()); + if (_in.has_duration()) + out.SetDuration(_in.duration().data()); + if (_in.has_emitting()) + out.SetEmitting(_in.emitting().data()); + if (_in.has_lifetime()) + out.SetLifetime(_in.lifetime().data()); + if (_in.has_scale_rate()) + out.SetScaleRate(_in.scale_rate().data()); + if (_in.has_color_range_image()) + out.SetColorRangeImage(_in.color_range_image().data()); + + for (int i = 0; i < _in.header().data_size(); ++i) + { + auto data = _in.header().data(i); + if (data.key() == "topic" && data.value_size() > 0) + { + out.SetTopic(data.value(0)); + } + } + + return out; +} diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 76fad52775..871bb6ddc4 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -773,3 +773,83 @@ TEST(Conversions, Actor) EXPECT_EQ(math::Pose3d(6, 5, 4, 0, 0, 0), newActor.TrajectoryByIndex(0)->WaypointByIndex(0)->Pose()); } + +///////////////////////////////////////////////// +TEST(Conversions, ParticleEmitter) +{ + sdf::ParticleEmitter emitter; + emitter.SetName("my_emitter"); + emitter.SetType(sdf::ParticleEmitterType::BOX); + emitter.SetEmitting(false); + emitter.SetDuration(12); + emitter.SetLifetime(56); + emitter.SetRate(0.5); + emitter.SetScaleRate(1.2); + emitter.SetMinVelocity(0.1); + emitter.SetMaxVelocity(0.2); + emitter.SetSize(math::Vector3d(1, 2, 3)); + emitter.SetParticleSize(math::Vector3d(4, 5, 6)); + emitter.SetColorStart(math::Color(0.1, 0.2, 0.3)); + emitter.SetColorEnd(math::Color(0.4, 0.5, 0.6)); + emitter.SetColorRangeImage("range_image"); + emitter.SetTopic("my_topic"); + emitter.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); + + sdf::Material material; + sdf::Pbr pbr; + sdf::PbrWorkflow workflow; + workflow.SetType(sdf::PbrWorkflowType::METAL); + workflow.SetAlbedoMap("albedo_map.png"); + pbr.SetWorkflow(workflow.Type(), workflow); + material.SetPbrMaterial(pbr); + + emitter.SetMaterial(material); + + // Convert SDF to a message. + msgs::ParticleEmitter emitterMsg = convert(emitter); + + EXPECT_EQ("my_emitter", emitterMsg.name()); + EXPECT_EQ(msgs::ParticleEmitter::BOX, emitterMsg.type()); + EXPECT_FALSE(emitterMsg.emitting().data()); + EXPECT_NEAR(12, emitterMsg.duration().data(), 1e-3); + EXPECT_NEAR(56, emitterMsg.lifetime().data(), 1e-3); + EXPECT_NEAR(0.5, emitterMsg.rate().data(), 1e-3); + EXPECT_NEAR(1.2, emitterMsg.scale_rate().data(), 1e-3); + EXPECT_NEAR(0.1, emitterMsg.min_velocity().data(), 1e-3); + EXPECT_NEAR(0.2, emitterMsg.max_velocity().data(), 1e-3); + EXPECT_EQ(math::Vector3d(1, 2, 3), msgs::Convert(emitterMsg.size())); + EXPECT_EQ(math::Vector3d(4, 5, 6), msgs::Convert(emitterMsg.particle_size())); + EXPECT_EQ(math::Color(0.1, 0.2, 0.3), + msgs::Convert(emitterMsg.color_start())); + EXPECT_EQ(math::Color(0.4, 0.5, 0.6), msgs::Convert(emitterMsg.color_end())); + EXPECT_EQ("range_image", emitterMsg.color_range_image().data()); + + auto header = emitterMsg.header().data(0); + EXPECT_EQ("topic", header.key()); + EXPECT_EQ("my_topic", header.value(0)); + + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), msgs::Convert(emitterMsg.pose())); + + auto pbrMsg = emitterMsg.material().pbr(); + EXPECT_EQ(msgs::Material::PBR::METAL, pbrMsg.type()); + EXPECT_EQ("albedo_map.png", pbrMsg.albedo_map()); + + // Convert the message back to SDF. + sdf::ParticleEmitter emitter2 = convert(emitterMsg); + EXPECT_EQ(emitter2.Name(), emitter.Name()); + EXPECT_EQ(emitter2.Type(), emitter.Type()); + EXPECT_EQ(emitter2.Emitting(), emitter.Emitting()); + EXPECT_NEAR(emitter2.Duration(), emitter.Duration(), 1e-3); + EXPECT_NEAR(emitter2.Lifetime(), emitter.Lifetime(), 1e-3); + EXPECT_NEAR(emitter2.Rate(), emitter.Rate(), 1e-3); + EXPECT_NEAR(emitter2.ScaleRate(), emitter.ScaleRate(), 1e-3); + EXPECT_NEAR(emitter2.MinVelocity(), emitter.MinVelocity(), 1e-3); + EXPECT_NEAR(emitter2.MaxVelocity(), emitter.MaxVelocity(), 1e-3); + EXPECT_EQ(emitter2.Size(), emitter.Size()); + EXPECT_EQ(emitter2.ParticleSize(), emitter.ParticleSize()); + EXPECT_EQ(emitter2.ColorStart(), emitter.ColorStart()); + EXPECT_EQ(emitter2.ColorEnd(), emitter.ColorEnd()); + EXPECT_EQ(emitter2.ColorRangeImage(), emitter.ColorRangeImage()); + EXPECT_EQ(emitter2.Topic(), emitter.Topic()); + EXPECT_EQ(emitter2.RawPose(), emitter.RawPose()); +} diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 6d8b35e951..4b8453c2ea 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -55,6 +55,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" @@ -458,6 +459,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) this->SetParent(sensorEntity, linkEntity); } + // Particle emitters + for (uint64_t emitterIndex = 0; emitterIndex < _link->ParticleEmitterCount(); + ++emitterIndex) + { + auto emitter = _link->ParticleEmitterByIndex(emitterIndex); + auto emitterEntity = this->CreateEntities(emitter); + + this->SetParent(emitterEntity, linkEntity); + } + return linkEntity; } @@ -548,6 +559,25 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) return visualEntity; } +////////////////////////////////////////////////// +Entity SdfEntityCreator::CreateEntities(const sdf::ParticleEmitter *_emitter) +{ + IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::ParticleEmitter)"); + + // Entity + Entity emitterEntity = this->dataPtr->ecm->CreateEntity(); + + // Components + this->dataPtr->ecm->CreateComponent(emitterEntity, + components::ParticleEmitter(convert(*_emitter))); + this->dataPtr->ecm->CreateComponent(emitterEntity, + components::Pose(ResolveSdfPose(_emitter->SemanticPose()))); + this->dataPtr->ecm->CreateComponent(emitterEntity, + components::Name(_emitter->Name())); + + return emitterEntity; +} + ////////////////////////////////////////////////// Entity SdfEntityCreator::CreateEntities(const sdf::Collision *_collision) { diff --git a/src/Util.cc b/src/Util.cc index 5f2e4740f9..d6af1281ea 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -43,6 +43,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" @@ -168,6 +169,10 @@ ComponentTypeId entityTypeId(const Entity &_entity, { type = components::Actor::typeId; } + else if (_ecm.Component(_entity)) + { + type = components::ParticleEmitter::typeId; + } return type; } @@ -214,6 +219,10 @@ std::string entityTypeStr(const Entity &_entity, { type = "actor"; } + else if (_ecm.Component(_entity)) + { + type = "particle_emitter"; + } return type; } @@ -435,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) ? "" : + 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 f9edf2543b..15e12002c9 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -28,6 +28,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -272,6 +273,10 @@ TEST_F(UtilTest, EntityTypeId) entity = ecm.CreateEntity(); ecm.CreateComponent(entity, components::Actor()); EXPECT_EQ(components::Actor::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::ParticleEmitter()); + EXPECT_EQ(components::ParticleEmitter::typeId, entityTypeId(entity, ecm)); } ///////////////////////////////////////////////// @@ -317,6 +322,10 @@ TEST_F(UtilTest, EntityTypeStr) entity = ecm.CreateEntity(); ecm.CreateComponent(entity, components::Actor()); EXPECT_EQ("actor", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::ParticleEmitter()); + EXPECT_EQ("particle_emitter", entityTypeStr(entity, ecm)); } ///////////////////////////////////////////////// @@ -510,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/CMakeLists.txt b/src/systems/CMakeLists.txt index b4054fcc35..6d12c5fc04 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -99,6 +99,7 @@ add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) add_subdirectory(optical_tactile_plugin) add_subdirectory(particle_emitter) +add_subdirectory(particle_emitter2) add_subdirectory(performer_detector) add_subdirectory(physics) add_subdirectory(pose_publisher) diff --git a/src/systems/particle_emitter/ParticleEmitter.hh b/src/systems/particle_emitter/ParticleEmitter.hh index 5b3df5e476..afa90cb293 100644 --- a/src/systems/particle_emitter/ParticleEmitter.hh +++ b/src/systems/particle_emitter/ParticleEmitter.hh @@ -33,6 +33,9 @@ namespace systems /// \brief A system for creating a particle emitter. /// + /// This system will be deprecated in Igition Fortress. Please consider + /// using the ParticleEmitter2 system. + /// /// System parameters /// /// ``: Unique name for the particle emitter. The name will be diff --git a/src/systems/particle_emitter2/CMakeLists.txt b/src/systems/particle_emitter2/CMakeLists.txt new file mode 100644 index 0000000000..3abcb6619d --- /dev/null +++ b/src/systems/particle_emitter2/CMakeLists.txt @@ -0,0 +1,9 @@ +gz_add_system(particle-emitter2 + SOURCES + ParticleEmitter2.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/particle_emitter2/ParticleEmitter2.cc b/src/systems/particle_emitter2/ParticleEmitter2.cc new file mode 100644 index 0000000000..84fe2ec90e --- /dev/null +++ b/src/systems/particle_emitter2/ParticleEmitter2.cc @@ -0,0 +1,264 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include "ParticleEmitter2.hh" + +using namespace std::chrono_literals; + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +// Private data class. +class ignition::gazebo::systems::ParticleEmitter2Private +{ + /// \brief Callback for receiving particle emitter commands. + /// \param[in] _msg Particle emitter message. + public: void OnCmd(const ignition::msgs::ParticleEmitter &_msg, + const transport::MessageInfo &_info); + + public: bool EmittersService(ignition::msgs::ParticleEmitter_V &_res); + + /// \brief The transport node. + public: ignition::transport::Node node; + + /// \brief Map of topic name to particle emitter entity. + public: std::map emitterTopicMap; + + /// \brief Map of Entity to particle emitter command requested externally. + public: std::map userCmd; + + /// \brief A mutex to protect the user command. + public: std::mutex mutex; + + /// \brief Used to coordinate the emitter service response. + public: std::condition_variable serviceCv; + + /// \brief Protects serviceMsg. + public: std::mutex serviceMutex; + + /// \brief Filled on demand for the emitter service. + public: msgs::ParticleEmitter_V serviceMsg; + + /// \brief True if the emitter service has been requested. + public: bool serviceRequest = false; + +}; + +////////////////////////////////////////////////// +void ParticleEmitter2Private::OnCmd(const msgs::ParticleEmitter &_msg, + const transport::MessageInfo &_info) +{ + std::lock_guard lock(this->mutex); + std::map::const_iterator iter = + this->emitterTopicMap.find(_info.Topic()); + if (iter != this->emitterTopicMap.end()) + { + this->userCmd[iter->second].CopyFrom(_msg); + } + else + { + ignwarn << "Topic[" << _info.Topic() << "] is not known to the particle " + "emitter system. The requested command will be ignored.\n"; + } +} + +////////////////////////////////////////////////// +bool ParticleEmitter2Private::EmittersService( + ignition::msgs::ParticleEmitter_V &_res) +{ + _res.Clear(); + + // Lock and wait for an iteration to be run and fill the state + std::unique_lock lock(this->serviceMutex); + + this->serviceRequest = true; + bool success = this->serviceCv.wait_for(lock, 5s, [&] + { + return !this->serviceRequest; + }); + + if (success) + _res.CopyFrom(this->serviceMsg); + else + ignerr << "Timed out waiting for state" << std::endl; + + return success; +} + +////////////////////////////////////////////////// +ParticleEmitter2::ParticleEmitter2() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void ParticleEmitter2::Configure(const Entity &_entity, + const std::shared_ptr & /*_sdf*/, + EntityComponentManager &_ecm, + EventManager & /*_eventMgr*/) +{ + // World + const components::Name *name = _ecm.Component(_entity); + if (name == nullptr) + { + ignerr << "World with id: " << _entity + << " has no name. ParticleEmitter2 cannot create transport topics\n"; + return; + } + + std::string emittersService = "/world/" + name->Data() + "/particle_emitters"; + if (this->dataPtr->node.Advertise(emittersService, + &ParticleEmitter2Private::EmittersService, this->dataPtr.get())) + { + ignmsg << "Serving particle emitter information on [" + << emittersService << "]" << std::endl; + } + else + { + ignerr << "Something went wrong, failed to advertise [" << emittersService + << "]" << std::endl; + } +} + +////////////////////////////////////////////////// +void ParticleEmitter2::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("ParticleEmitter2::PreUpdate"); + + std::lock_guard lock(this->dataPtr->mutex); + + // Create particle emitters + _ecm.EachNew( + [&](const Entity &_entity, + const components::ParticleEmitter *_emitter)->bool + { + std::string topic; + + // Get the topic information from the header, which is currently a + // hack to avoid breaking the particle_emitter.proto message. + if (_emitter->Data().has_header()) + { + for (const auto data : _emitter->Data().header().data()) + { + if (data.key() == "topic" && !data.value().empty()) + { + topic = data.value(0); + } + } + } + + // If a topic has not been specified, then generate topic based + // 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( + topic, &ParticleEmitter2Private::OnCmd, this->dataPtr.get())) + { + ignerr << "Error subscribing to topic [" << topic << "]. " + << "Particle emitter will not receive updates." << std::endl; + return false; + } + ignmsg << "Particle emitter[" + << scopedName(_entity, _ecm, "::", false) << "] subscribed " + << "to command messages on topic[" << topic << "]\n"; + + // Store the topic name so that we can apply user commands + // correctly. + this->dataPtr->emitterTopicMap[topic] = _entity; + return true; + }); + + // Populate teh service message on demand + if (this->dataPtr->serviceRequest) + { + std::unique_lock lockCv(this->dataPtr->serviceMutex); + this->dataPtr->serviceMsg.Clear(); + + // Get all the particle emitters + _ecm.Each([&](const Entity & /*_entity*/, + const components::ParticleEmitter *_emitter)->bool + { + msgs::ParticleEmitter *emitterMsg = + this->dataPtr->serviceMsg.add_particle_emitter(); + emitterMsg->CopyFrom(_emitter->Data()); + return true; + }); + this->dataPtr->serviceRequest = false; + this->dataPtr->serviceCv.notify_all(); + } + + if (this->dataPtr->userCmd.empty() || _info.paused) + return; + + + // Process each command + for (const auto cmd : this->dataPtr->userCmd) + { + // Create component. + auto emitterComp = _ecm.Component( + cmd.first); + if (!emitterComp) + { + _ecm.CreateComponent(cmd.first, + components::ParticleEmitterCmd(cmd.second)); + } + else + { + emitterComp->Data() = cmd.second; + + // Note: we process the cmd component in RenderUtil but if there is only + // rendering on the gui side, it will not be able to remove the cmd + // component from the ECM. It seems like adding OneTimeChange here will + // make sure the cmd component is found again in Each call on GUI side. + // todo(anyone) find a better way to process this cmd component in + // RenderUtil.cc + _ecm.SetChanged(cmd.first, + components::ParticleEmitterCmd::typeId, + ComponentState::OneTimeChange); + } + + } + this->dataPtr->userCmd.clear(); +} + +IGNITION_ADD_PLUGIN(ParticleEmitter2, + ignition::gazebo::System, + ParticleEmitter2::ISystemConfigure, + ParticleEmitter2::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ParticleEmitter2, + "ignition::gazebo::systems::ParticleEmitter2") diff --git a/src/systems/particle_emitter2/ParticleEmitter2.hh b/src/systems/particle_emitter2/ParticleEmitter2.hh new file mode 100644 index 0000000000..9d136899e4 --- /dev/null +++ b/src/systems/particle_emitter2/ParticleEmitter2.hh @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_PARTICLE_EMITTER2_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PARTICLE_EMITTER2_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + class ParticleEmitter2Private; + + /// \brief A system for running and managing particle emitters. A particle + /// emitter is defined using the SDF element. + /// + /// This system will create a transport subscriber for each + /// using the child name. If a is not + /// specified, the following topic naming scheme will be used: + /// `/model/{model_name}/link/{link_name}/particle_emitter/{emitter_name}/cmd` + /// + /// \todo(nkoenig) Plan for ParticleEmitter and ParticleEmitter2: + /// 1. Deprecate ParticleEmitter in Ignition Fortress. + /// 2. Remove ParticleEmitter in Ignition G. + /// 3. Copy ParticleEmitter2 to ParticleEmitter in Ignition G. + /// 4. Deprecrate ParticleEmitter2 in Ignition G. + /// 5. Remove ParticleEmitter2 in Ignition H. + class IGNITION_GAZEBO_VISIBLE ParticleEmitter2 + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: ParticleEmitter2(); + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif + diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 683b7a9472..59a3c003a9 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -37,6 +37,7 @@ set(tests multiple_servers.cc network_handshake.cc particle_emitter.cc + particle_emitter2.cc performer_detector.cc physics_system.cc play_pause.cc diff --git a/test/integration/particle_emitter2.cc b/test/integration/particle_emitter2.cc new file mode 100644 index 0000000000..c88f9a9ce2 --- /dev/null +++ b/test/integration/particle_emitter2.cc @@ -0,0 +1,162 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/test_config.hh" + +#include "helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; + +class ParticleEmitter2Test : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } + public: void LoadWorld(const std::string &_path, bool _useLevels = false) + { + this->serverConfig.SetSdfFile( + common::joinPaths(PROJECT_SOURCE_PATH, _path)); + this->serverConfig.SetUseLevels(_useLevels); + + this->server = std::make_unique(this->serverConfig); + EXPECT_FALSE(this->server->Running()); + EXPECT_FALSE(*this->server->Running(0)); + using namespace std::chrono_literals; + this->server->SetUpdatePeriod(1ns); + } + + public: ServerConfig serverConfig; + public: std::unique_ptr server; +}; + +///////////////////////////////////////////////// +// Load an SDF with a particle emitter and verify its properties. +TEST_F(ParticleEmitter2Test, SDFLoad) +{ + bool updateCustomChecked{false}; + bool updateDefaultChecked{false}; + + this->LoadWorld("test/worlds/particle_emitter2.sdf"); + + // Create a system that checks a particle emitter. + test::Relay testSystem; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::ParticleEmitter *_emitter, + const components::Name *_name, + const components::Pose *_pose) -> bool + { + + if (_name->Data() == "smoke_emitter") + { + updateCustomChecked = true; + + EXPECT_EQ("smoke_emitter", _name->Data()); + EXPECT_EQ(_name->Data(), _emitter->Data().name()); + EXPECT_EQ(msgs::ParticleEmitter_EmitterType_BOX, + _emitter->Data().type()); + EXPECT_EQ(math::Pose3d(0, 1, 0, 0, 0, 0), _pose->Data()); + EXPECT_EQ(_pose->Data(), + msgs::Convert(_emitter->Data().pose())); + EXPECT_EQ(math::Vector3d(2, 2, 2), + msgs::Convert(_emitter->Data().size())); + EXPECT_DOUBLE_EQ(5.0, _emitter->Data().rate().data()); + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().duration().data()); + EXPECT_TRUE(_emitter->Data().emitting().data()); + EXPECT_EQ(math::Vector3d(3, 3, 3), + msgs::Convert(_emitter->Data().particle_size())); + EXPECT_DOUBLE_EQ(2.0, _emitter->Data().lifetime().data()); + // TODO(anyone) add material check here + EXPECT_DOUBLE_EQ(10.0, + _emitter->Data().min_velocity().data()); + EXPECT_DOUBLE_EQ(20.0, + _emitter->Data().max_velocity().data()); + EXPECT_EQ(math::Color::Blue, + msgs::Convert(_emitter->Data().color_start())); + EXPECT_EQ(math::Color::Green, + msgs::Convert(_emitter->Data().color_end())); + EXPECT_DOUBLE_EQ(10.0, _emitter->Data().scale_rate().data()); + + // color range image is empty because the emitter system + // will not be able to find a file that does not exist + EXPECT_EQ("/path/to/dummy_image.png", + _emitter->Data().color_range_image().data()); + } + else + { + updateDefaultChecked = true; + + EXPECT_TRUE(_name->Data().find(std::to_string(_entity)) + != std::string::npos); + EXPECT_EQ(_name->Data(), _emitter->Data().name()); + EXPECT_EQ(msgs::ParticleEmitter_EmitterType_POINT, + _emitter->Data().type()); + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 0), _pose->Data()); + EXPECT_EQ(_pose->Data(), + msgs::Convert(_emitter->Data().pose())); + EXPECT_EQ(math::Vector3d(1, 1, 1), + msgs::Convert(_emitter->Data().size())); + EXPECT_DOUBLE_EQ(10.0, _emitter->Data().rate().data()); + EXPECT_DOUBLE_EQ(0.0, _emitter->Data().duration().data()); + EXPECT_FALSE(_emitter->Data().emitting().data()); + EXPECT_EQ(math::Vector3d(1, 1, 1), + msgs::Convert(_emitter->Data().particle_size())); + EXPECT_DOUBLE_EQ(5.0, _emitter->Data().lifetime().data()); + // TODO(anyone) add material check here + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().min_velocity().data()); + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().max_velocity().data()); + EXPECT_EQ(math::Color::White, + msgs::Convert(_emitter->Data().color_start())); + EXPECT_EQ(math::Color::White, + msgs::Convert(_emitter->Data().color_end())); + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().scale_rate().data()); + EXPECT_EQ("", _emitter->Data().color_range_image().data()); + } + + return true; + }); + }); + + this->server->AddSystem(testSystem.systemPtr); + this->server->Run(true, 1, false); + + EXPECT_TRUE(updateCustomChecked); + EXPECT_TRUE(updateDefaultChecked); +} diff --git a/test/worlds/particle_emitter2.sdf b/test/worlds/particle_emitter2.sdf new file mode 100644 index 0000000000..7a8781512b --- /dev/null +++ b/test/worlds/particle_emitter2.sdf @@ -0,0 +1,150 @@ + + + + + + 0.001 + 1.0 + + + + + + + + + + + 3D View + false + false + 0 + + + + + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -1 0 1 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + + + + true + true + true + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0 0 0 0 0 0 + true + + + + + 1 1 1 + + + + + 0 1 0 0 0 0 + 2 2 2 + 5 + 1 + true + 3 3 3 + 2 + + 10 + 20 + 0 0 1 + 0 1 0 + 10 + /path/to/dummy_image.png + + + + + + + + 5 5 0 0 0 0 + true + + + + + 1 1 1 + + + + + + + + + + + +