Patch particle emitter2 service (#777)

* Patch particle emitter2 service

Signed-off-by: Nate Koenig <[email protected]>

* Remove condition variable

Signed-off-by: Nate Koenig <[email protected]>

* Set emitter frame and relative pose

Signed-off-by: Nate Koenig <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>

src/systems/particle_emitter2/ParticleEmitter2.cc

@@ -29,7 +29,9 @@
 #include <ignition/transport/Node.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/Util.hh>
 #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<std::mutex> 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<std::mutex> lock(this->serviceMutex);
+ _res.CopyFrom(this->serviceMsg);
+ return true;
 }
 
 //////////////////////////////////////////////////
@@ -160,71 +143,80 @@ void ParticleEmitter2::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
 
  // Create particle emitters
- _ecm.EachNew<components::ParticleEmitter>(
- [&](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<std::mutex> serviceLock(this->dataPtr->serviceMutex);
+ _ecm.EachNew<components::ParticleEmitter, components::ParentEntity,
+ components::Pose>(
+ [&](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<std::mutex> 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<components::ParticleEmitter>([&](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)
  {