diff --git a/src/systems/particle_emitter2/ParticleEmitter2.cc b/src/systems/particle_emitter2/ParticleEmitter2.cc index 84fe2ec90e..0298e71530 100644 --- a/src/systems/particle_emitter2/ParticleEmitter2.cc +++ b/src/systems/particle_emitter2/ParticleEmitter2.cc @@ -29,7 +29,9 @@ #include #include +#include #include +#include #include #include "ParticleEmitter2.hh" @@ -61,18 +63,11 @@ class ignition::gazebo::systems::ParticleEmitter2Private /// \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; - }; ////////////////////////////////////////////////// @@ -99,21 +94,9 @@ bool ParticleEmitter2Private::EmittersService( { _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; + std::scoped_lock lock(this->serviceMutex); + _res.CopyFrom(this->serviceMsg); + return true; } ////////////////////////////////////////////////// @@ -160,71 +143,80 @@ void ParticleEmitter2::PreUpdate(const ignition::gazebo::UpdateInfo &_info, 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()) + { + std::lock_guard serviceLock(this->dataPtr->serviceMutex); + _ecm.EachNew( + [&](const Entity &_entity, + const components::ParticleEmitter *_emitter, + const components::ParentEntity *_parent, + const components::Pose *_pose)->bool { - for (const auto data : _emitter->Data().header().data()) + 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()) { - if (data.key() == "topic" && !data.value().empty()) + for (const auto data : _emitter->Data().header().data()) { - topic = data.value(0); + 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"; + // 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(); + // 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"; - // Get all the particle emitters - _ecm.Each([&](const Entity & /*_entity*/, - const components::ParticleEmitter *_emitter)->bool - { + // Store the topic name so that we can apply user commands + // correctly. + this->dataPtr->emitterTopicMap[topic] = _entity; + + // Store the emitter information in the service message, which + // can then be used in the particle_emitters service. msgs::ParticleEmitter *emitterMsg = this->dataPtr->serviceMsg.add_particle_emitter(); emitterMsg->CopyFrom(_emitter->Data()); + msgs::Set(emitterMsg->mutable_pose(), _pose->Data()); + + // Set the topic information if it was not set via SDF. + if (!emitterMsg->has_header()) + { + auto headerData = emitterMsg->mutable_header()->add_data(); + headerData->set_key("topic"); + headerData->add_value(topic); + } + + // Set the particle emitter frame + auto frameData = emitterMsg->mutable_header()->add_data(); + frameData->set_key("frame"); + frameData->add_value( + removeParentScope( + scopedName(_parent->Data(), _ecm, "::", false), "::")); + 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) {