diff --git a/examples/worlds/particle_emitter.sdf b/examples/worlds/particle_emitter.sdf
new file mode 100644
index 0000000000..58fb36c3be
--- /dev/null
+++ b/examples/worlds/particle_emitter.sdf
@@ -0,0 +1,90 @@
+
+
+
+
+
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+
+
+
+
+ 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/caguero/models/smoke_generator
+
+
+
+
+
+
diff --git a/include/ignition/gazebo/components/ParticleEmitter.hh b/include/ignition/gazebo/components/ParticleEmitter.hh
index a6ec2a242c..86fc36bd88 100644
--- a/include/ignition/gazebo/components/ParticleEmitter.hh
+++ b/include/ignition/gazebo/components/ParticleEmitter.hh
@@ -37,6 +37,13 @@ namespace components
serializers::MsgSerializer>;
IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ParticleEmitter",
ParticleEmitter)
+
+ /// \brief A component that contains a particle emitter command.
+ using ParticleEmitterCmd = Component;
+ IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ParticleEmitterCmd",
+ ParticleEmitterCmd)
}
}
}
diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh
index 113c8731a8..f7643e250f 100644
--- a/include/ignition/gazebo/rendering/SceneManager.hh
+++ b/include/ignition/gazebo/rendering/SceneManager.hh
@@ -34,6 +34,8 @@
#include
#include
+#include
+
#include
#include
@@ -161,6 +163,21 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
public: rendering::LightPtr CreateLight(Entity _id,
const sdf::Light &_light, Entity _parentId);
+ /// \brief Create a particle emitter.
+ /// \param[in] _id Unique particle emitter id
+ /// \param[in] _emitter Particle emitter data
+ /// \param[in] _parentId Parent id
+ /// \return Default particle emitter object created
+ public: rendering::ParticleEmitterPtr CreateParticleEmitter(
+ Entity _id, const msgs::ParticleEmitter &_emitter, Entity _parentId);
+
+ /// \brief Update an existing particle emitter
+ /// \brief _id Emitter id to update
+ /// \brief _emitter Data to update the particle emitter
+ /// \return Particle emitter updated
+ public: rendering::ParticleEmitterPtr UpdateParticleEmitter(Entity _id,
+ const msgs::ParticleEmitter &_emitter);
+
/// \brief Ignition sensors is the one responsible for adding sensors
/// to the scene. Here we just keep track of it and make sure it has
/// the correct parent.
diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc
index b22da15d99..e43b749893 100644
--- a/src/gui/plugins/scene3d/Scene3D.cc
+++ b/src/gui/plugins/scene3d/Scene3D.cc
@@ -2752,6 +2752,7 @@ void Scene3D::Update(const UpdateInfo &_info,
msgs::Pose poseMsg = msgs::Convert(renderWindow->CameraPose());
this->dataPtr->cameraPosePub.Publish(poseMsg);
}
+ this->dataPtr->renderUtil->UpdateECM(_info, _ecm);
this->dataPtr->renderUtil->UpdateFromECM(_info, _ecm);
// check if video recording is enabled and if we need to lock step
diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
index c1c507713b..d1f833d12d 100644
--- a/src/rendering/RenderUtil.cc
+++ b/src/rendering/RenderUtil.cc
@@ -42,6 +42,8 @@
#include
#include
+#include
+
#include
#include
#include
@@ -60,6 +62,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/RgbdCamera.hh"
#include "ignition/gazebo/components/Scene.hh"
@@ -163,6 +166,17 @@ class ignition::gazebo::RenderUtilPrivate
public: std::vector>
newSensors;
+ /// \brief New particle emitter to be created. The elements in the tuple are:
+ /// [0] entity id, [1], particle emitter, [2] parent entity id
+ public: std::vector>
+ newParticleEmitters;
+
+ /// \brief New particle emitter commands to be requested.
+ /// The map key and value are: entity id of the particle emitter to
+ /// update, and particle emitter msg
+ public: std::unordered_map
+ newParticleEmittersCmds;
+
/// \brief Map of ids of entites to be removed and sim iteration when the
/// remove request is received
public: std::unordered_map removeEntities;
@@ -289,6 +303,28 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/,
EntityComponentManager &_ecm)
{
std::lock_guard lock(this->dataPtr->updateMutex);
+
+ // particle emitters commands
+ _ecm.Each(
+ [&](const Entity &_entity,
+ const components::ParticleEmitterCmd *_emitterCmd) -> bool
+ {
+ // store emitter properties and update them in rendering thread
+ this->dataPtr->newParticleEmittersCmds[_entity] =
+ _emitterCmd->Data();
+
+ // update pose comp here
+ if (_emitterCmd->Data().has_pose())
+ {
+ auto poseComp = _ecm.Component(_entity);
+ if (poseComp)
+ poseComp->Data() = msgs::Convert(_emitterCmd->Data().pose());
+ }
+ _ecm.RemoveComponent(_entity);
+
+ return true;
+ });
+
// Update thermal cameras
_ecm.Each(
[&](const Entity &_entity,
@@ -413,6 +449,9 @@ void RenderUtil::Update()
auto newVisuals = std::move(this->dataPtr->newVisuals);
auto newActors = std::move(this->dataPtr->newActors);
auto newLights = std::move(this->dataPtr->newLights);
+ auto newParticleEmitters = std::move(this->dataPtr->newParticleEmitters);
+ auto newParticleEmittersCmds =
+ std::move(this->dataPtr->newParticleEmittersCmds);
auto removeEntities = std::move(this->dataPtr->removeEntities);
auto entityPoses = std::move(this->dataPtr->entityPoses);
auto trajectoryPoses = std::move(this->dataPtr->trajectoryPoses);
@@ -428,6 +467,8 @@ void RenderUtil::Update()
this->dataPtr->newVisuals.clear();
this->dataPtr->newActors.clear();
this->dataPtr->newLights.clear();
+ this->dataPtr->newParticleEmitters.clear();
+ this->dataPtr->newParticleEmittersCmds.clear();
this->dataPtr->removeEntities.clear();
this->dataPtr->entityPoses.clear();
this->dataPtr->trajectoryPoses.clear();
@@ -529,6 +570,18 @@ void RenderUtil::Update()
std::get<0>(light), std::get<1>(light), std::get<2>(light));
}
+ for (const auto &emitter : newParticleEmitters)
+ {
+ this->dataPtr->sceneManager.CreateParticleEmitter(
+ std::get<0>(emitter), std::get<1>(emitter), std::get<2>(emitter));
+ }
+
+ for (const auto &emitterCmd : newParticleEmittersCmds)
+ {
+ this->dataPtr->sceneManager.UpdateParticleEmitter(
+ emitterCmd.first, emitterCmd.second);
+ }
+
if (this->dataPtr->enableSensors && this->dataPtr->createSensorCb)
{
for (const auto &sensor : newSensors)
@@ -1013,6 +1066,17 @@ void RenderUtilPrivate::CreateRenderingEntities(
return true;
});
+ // particle emitters
+ _ecm.Each(
+ [&](const Entity &_entity,
+ const components::ParticleEmitter *_emitter,
+ const components::ParentEntity *_parent) -> bool
+ {
+ this->newParticleEmitters.push_back(
+ std::make_tuple(_entity, _emitter->Data(), _parent->Data()));
+ return true;
+ });
+
if (this->enableSensors)
{
// Create cameras
@@ -1224,6 +1288,17 @@ void RenderUtilPrivate::CreateRenderingEntities(
return true;
});
+ // particle emitters
+ _ecm.EachNew(
+ [&](const Entity &_entity,
+ const components::ParticleEmitter *_emitter,
+ const components::ParentEntity *_parent) -> bool
+ {
+ this->newParticleEmitters.push_back(
+ std::make_tuple(_entity, _emitter->Data(), _parent->Data()));
+ return true;
+ });
+
if (this->enableSensors)
{
// Create cameras
@@ -1465,6 +1540,14 @@ void RenderUtilPrivate::RemoveRenderingEntities(
return true;
});
+ // particle emitters
+ _ecm.EachRemoved(
+ [&](const Entity &_entity, const components::ParticleEmitter *)->bool
+ {
+ this->removeEntities[_entity] = _info.iterations;
+ return true;
+ });
+
// cameras
_ecm.EachRemoved(
[&](const Entity &_entity, const components::Camera *)->bool
diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc
index 9bdfe0b1bb..c4e603d430 100644
--- a/src/rendering/SceneManager.cc
+++ b/src/rendering/SceneManager.cc
@@ -29,16 +29,20 @@
#include
#include
#include
+#include
#include
#include
-#include
+
+#include
#include
#include
#include
+#include
#include
#include
+#include "ignition/gazebo/Conversions.hh"
#include "ignition/gazebo/Util.hh"
#include "ignition/gazebo/rendering/SceneManager.hh"
@@ -75,6 +79,10 @@ class ignition::gazebo::SceneManagerPrivate
/// \brief Map of light entity in Gazebo to light pointers.
public: std::map lights;
+ /// \brief Map of particle emitter entity in Gazebo to particle emitter
+ /// rendering pointers.
+ public: std::map particleEmitters;
+
/// \brief Map of sensor entity in Gazebo to sensor pointers.
public: std::map sensors;
@@ -960,6 +968,148 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id,
return light;
}
+/////////////////////////////////////////////////
+rendering::ParticleEmitterPtr SceneManager::CreateParticleEmitter(
+ Entity _id, const msgs::ParticleEmitter &_emitter, Entity _parentId)
+{
+ if (!this->dataPtr->scene)
+ return rendering::ParticleEmitterPtr();
+
+ if (this->dataPtr->particleEmitters.find(_id) !=
+ this->dataPtr->particleEmitters.end())
+ {
+ ignerr << "Particle emitter with Id: [" << _id << "] already exists in the "
+ <<" scene" << std::endl;
+ return rendering::ParticleEmitterPtr();
+ }
+
+ rendering::VisualPtr parent;
+ if (_parentId != this->dataPtr->worldId)
+ {
+ auto it = this->dataPtr->visuals.find(_parentId);
+ if (it == this->dataPtr->visuals.end())
+ {
+ // It is possible to get here if the model entity is created then
+ // removed in between render updates.
+ return rendering::ParticleEmitterPtr();
+ }
+ parent = it->second;
+ }
+
+ // Name.
+ std::string name = _emitter.name().empty() ? std::to_string(_id) :
+ _emitter.name();
+ if (parent)
+ name = parent->Name() + "::" + name;
+
+ rendering::ParticleEmitterPtr emitter;
+ emitter = this->dataPtr->scene->CreateParticleEmitter(name);
+
+ this->dataPtr->particleEmitters[_id] = emitter;
+
+ if (parent)
+ parent->AddChild(emitter);
+
+ this->UpdateParticleEmitter(_id, _emitter);
+
+ return emitter;
+}
+
+/////////////////////////////////////////////////
+rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id,
+ const msgs::ParticleEmitter &_emitter)
+{
+ if (!this->dataPtr->scene)
+ return rendering::ParticleEmitterPtr();
+
+ // Sanity check: Make sure that the id exists.
+ auto emitterIt = this->dataPtr->particleEmitters.find(_id);
+ if (emitterIt == this->dataPtr->particleEmitters.end())
+ {
+ ignerr << "Particle emitter with Id: [" << _id << "] not found in the "
+ << "scene" << std::endl;
+ return rendering::ParticleEmitterPtr();
+ }
+ auto emitter = emitterIt->second;
+
+ // Type.
+ switch (_emitter.type())
+ {
+ case ignition::msgs::ParticleEmitter_EmitterType_BOX:
+ {
+ emitter->SetType(ignition::rendering::EmitterType::EM_BOX);
+ break;
+ }
+ case ignition::msgs::ParticleEmitter_EmitterType_CYLINDER:
+ {
+ emitter->SetType(ignition::rendering::EmitterType::EM_CYLINDER);
+ break;
+ }
+ case ignition::msgs::ParticleEmitter_EmitterType_ELLIPSOID:
+ {
+ emitter->SetType(ignition::rendering::EmitterType::EM_ELLIPSOID);
+ break;
+ }
+ default:
+ {
+ emitter->SetType(ignition::rendering::EmitterType::EM_POINT);
+ }
+ }
+
+ // Emitter size.
+ if (_emitter.has_size())
+ emitter->SetEmitterSize(ignition::msgs::Convert(_emitter.size()));
+
+ // Rate.
+ emitter->SetRate(_emitter.rate());
+
+ // Duration.
+ emitter->SetDuration(_emitter.duration());
+
+ // Emitting.
+ emitter->SetEmitting(_emitter.emitting());
+
+ // Particle size.
+ emitter->SetParticleSize(
+ ignition::msgs::Convert(_emitter.particle_size()));
+
+ // Lifetime.
+ emitter->SetLifetime(_emitter.lifetime());
+
+ // Material.
+ if (_emitter.has_material())
+ {
+ ignition::rendering::MaterialPtr material =
+ this->LoadMaterial(convert(_emitter.material()));
+ emitter->SetMaterial(material);
+ }
+
+ // Velocity range.
+ emitter->SetVelocityRange(_emitter.min_velocity(), _emitter.max_velocity());
+
+ // Color range image.
+ if (!_emitter.color_range_image().empty())
+ {
+ emitter->SetColorRangeImage(_emitter.color_range_image());
+ }
+ // Color range.
+ else if (_emitter.has_color_start() && _emitter.has_color_end())
+ {
+ emitter->SetColorRange(
+ ignition::msgs::Convert(_emitter.color_start()),
+ ignition::msgs::Convert(_emitter.color_end()));
+ }
+
+ // Scale rate.
+ emitter->SetScaleRate(_emitter.scale_rate());
+
+ // pose
+ if (_emitter.has_pose())
+ emitter->SetLocalPose(msgs::Convert(_emitter.pose()));
+
+ return emitter;
+}
+
/////////////////////////////////////////////////
bool SceneManager::AddSensor(Entity _gazeboId, const std::string &_sensorName,
Entity _parentGazeboId)
@@ -1010,6 +1160,8 @@ bool SceneManager::HasEntity(Entity _id) const
return this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end() ||
this->dataPtr->actors.find(_id) != this->dataPtr->actors.end() ||
this->dataPtr->lights.find(_id) != this->dataPtr->lights.end() ||
+ this->dataPtr->particleEmitters.find(_id) !=
+ this->dataPtr->particleEmitters.end() ||
this->dataPtr->sensors.find(_id) != this->dataPtr->sensors.end();
}
@@ -1021,22 +1173,22 @@ rendering::NodePtr SceneManager::NodeById(Entity _id) const
{
return vIt->second;
}
- else
+ auto lIt = this->dataPtr->lights.find(_id);
+ if (lIt != this->dataPtr->lights.end())
{
- auto lIt = this->dataPtr->lights.find(_id);
- if (lIt != this->dataPtr->lights.end())
- {
- return lIt->second;
- }
- else
- {
- auto sIt = this->dataPtr->sensors.find(_id);
- if (sIt != this->dataPtr->sensors.end())
- {
- return sIt->second;
- }
- }
+ return lIt->second;
}
+ auto sIt = this->dataPtr->sensors.find(_id);
+ if (sIt != this->dataPtr->sensors.end())
+ {
+ return sIt->second;
+ }
+ auto pIt = this->dataPtr->particleEmitters.find(_id);
+ if (pIt != this->dataPtr->particleEmitters.end())
+ {
+ return pIt->second;
+ }
+
return rendering::NodePtr();
}
@@ -1258,6 +1410,16 @@ void SceneManager::RemoveEntity(Entity _id)
}
}
+ {
+ auto it = this->dataPtr->particleEmitters.find(_id);
+ if (it != this->dataPtr->particleEmitters.end())
+ {
+ this->dataPtr->scene->DestroyVisual(it->second);
+ this->dataPtr->particleEmitters.erase(it);
+ return;
+ }
+ }
+
{
auto it = this->dataPtr->sensors.find(_id);
if (it != this->dataPtr->sensors.end())
diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
index 07b88dfba4..ff95341dac 100644
--- a/src/systems/CMakeLists.txt
+++ b/src/systems/CMakeLists.txt
@@ -97,6 +97,7 @@ add_subdirectory(magnetometer)
add_subdirectory(multicopter_motor_model)
add_subdirectory(multicopter_control)
add_subdirectory(optical_tactile_plugin)
+add_subdirectory(particle_emitter)
add_subdirectory(performer_detector)
add_subdirectory(physics)
add_subdirectory(pose_publisher)
diff --git a/src/systems/particle_emitter/CMakeLists.txt b/src/systems/particle_emitter/CMakeLists.txt
new file mode 100644
index 0000000000..9e75a90abf
--- /dev/null
+++ b/src/systems/particle_emitter/CMakeLists.txt
@@ -0,0 +1,9 @@
+gz_add_system(particle-emitter
+ SOURCES
+ ParticleEmitter.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_emitter/ParticleEmitter.cc b/src/systems/particle_emitter/ParticleEmitter.cc
new file mode 100644
index 0000000000..072bd50dd2
--- /dev/null
+++ b/src/systems/particle_emitter/ParticleEmitter.cc
@@ -0,0 +1,335 @@
+/*
+ * 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
+#include
+#include
+#include
+#include
+#include "ParticleEmitter.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace systems;
+
+// Private data class.
+class ignition::gazebo::systems::ParticleEmitterPrivate
+{
+ /// \brief Callback for receiving particle emitter commands.
+ /// \param[in] _msg Particle emitter message.
+ public: void OnCmd(const ignition::msgs::ParticleEmitter &_msg);
+
+ /// \brief The particle emitter parsed from SDF.
+ public: ignition::msgs::ParticleEmitter emitter;
+
+ /// \brief The transport node.
+ public: ignition::transport::Node node;
+
+ /// \brief Particle emitter entity.
+ public: Entity emitterEntity{kNullEntity};
+
+ /// \brief The particle emitter command requested externally.
+ public: ignition::msgs::ParticleEmitter userCmd;
+
+ public: bool newDataReceived = false;
+
+ /// \brief A mutex to protect the user command.
+ public: std::mutex mutex;
+};
+
+//////////////////////////////////////////////////
+void ParticleEmitterPrivate::OnCmd(const msgs::ParticleEmitter &_msg)
+{
+ std::lock_guard lock(this->mutex);
+ this->userCmd = _msg;
+ this->newDataReceived = true;
+}
+
+//////////////////////////////////////////////////
+ParticleEmitter::ParticleEmitter()
+ : System(), dataPtr(std::make_unique())
+{
+}
+
+//////////////////////////////////////////////////
+void ParticleEmitter::Configure(const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &_eventMgr)
+{
+ Model model = Model(_entity);
+
+ if (!model.Valid(_ecm))
+ {
+ ignerr << "ParticleEmitter plugin should be attached to a model entity. "
+ << "Failed to initialize." << std::endl;
+ return;
+ }
+
+ // Create a particle emitter entity.
+ this->dataPtr->emitterEntity = _ecm.CreateEntity();
+ if (this->dataPtr->emitterEntity == kNullEntity)
+ {
+ ignerr << "Failed to create a particle emitter entity" << std::endl;
+ return;
+ }
+
+ // allow_renaming
+ bool allowRenaming = false;
+ if (_sdf->HasElement("allow_renaming"))
+ allowRenaming = _sdf->Get("allow_renaming");
+
+ // Name.
+ std::string name = "particle_emitter_entity_" +
+ std::to_string(this->dataPtr->emitterEntity);
+ if (_sdf->HasElement("emitter_name"))
+ {
+ std::set emitterNames;
+ std::string emitterName = _sdf->Get("emitter_name");
+
+ // check to see if name is already taken
+ _ecm.Each(
+ [&emitterNames](const Entity &, const components::Name *_name,
+ const components::ParticleEmitter *)
+ {
+ emitterNames.insert(_name->Data());
+ return true;
+ });
+
+ name = emitterName;
+
+ // rename emitter if needed
+ if (emitterNames.find(emitterName) != emitterNames.end())
+ {
+ if (!allowRenaming)
+ {
+ ignwarn << "Entity named [" << name
+ << "] already exists and "
+ << "[allow_renaming] is false. Entity not spawned."
+ << std::endl;
+ return;
+ }
+ int counter = 0;
+ while (emitterNames.find(name) != emitterNames.end())
+ {
+ name = emitterName + "_" + std::to_string(++counter);
+ }
+ ignmsg << "Entity named [" << emitterName
+ << "] already exists. Renaming it to " << name << std::endl;
+ }
+ }
+ this->dataPtr->emitter.set_name(name);
+
+ // Type. The default type is point.
+ this->dataPtr->emitter.set_type(
+ ignition::msgs::ParticleEmitter_EmitterType_POINT);
+ std::string type = _sdf->Get("type", "point").first;
+ if (type == "box")
+ {
+ this->dataPtr->emitter.set_type(
+ ignition::msgs::ParticleEmitter_EmitterType_BOX);
+ }
+ else if (type == "cylinder")
+ {
+ this->dataPtr->emitter.set_type(
+ ignition::msgs::ParticleEmitter_EmitterType_CYLINDER);
+ }
+ else if (type == "ellipsoid")
+ {
+ this->dataPtr->emitter.set_type(
+ ignition::msgs::ParticleEmitter_EmitterType_ELLIPSOID);
+ }
+ else if (type != "point")
+ {
+ ignerr << "Unknown emitter type [" << type << "]. Using [point] instead"
+ << std::endl;
+ }
+
+ // Pose.
+ ignition::math::Pose3d pose =
+ _sdf->Get("pose");
+ ignition::msgs::Set(this->dataPtr->emitter.mutable_pose(), pose);
+
+ // Size.
+ ignition::math::Vector3d size = ignition::math::Vector3d::One;
+ if (_sdf->HasElement("size"))
+ size = _sdf->Get("size");
+ ignition::msgs::Set(this->dataPtr->emitter.mutable_size(), size);
+
+ // Rate.
+ this->dataPtr->emitter.set_rate(_sdf->Get("rate", 10).first);
+
+ // Duration.
+ this->dataPtr->emitter.set_duration(_sdf->Get("duration", 0).first);
+
+ // Emitting.
+ this->dataPtr->emitter.set_emitting(_sdf->Get("emitting", false).first);
+
+ // Particle size.
+ size = ignition::math::Vector3d::One;
+ if (_sdf->HasElement("particle_size"))
+ size = _sdf->Get("particle_size");
+ ignition::msgs::Set(this->dataPtr->emitter.mutable_particle_size(), size);
+
+ // Lifetime.
+ this->dataPtr->emitter.set_lifetime(_sdf->Get("lifetime", 5).first);
+
+ // Material.
+ if (_sdf->HasElement("material"))
+ {
+ auto materialElem = _sdf->GetElementImpl("material");
+ sdf::Material material;
+ material.Load(materialElem);
+ ignition::msgs::Material materialMsg = convert(material);
+ this->dataPtr->emitter.mutable_material()->CopyFrom(materialMsg);
+ }
+
+ // Min velocity.
+ this->dataPtr->emitter.set_min_velocity(
+ _sdf->Get("min_velocity", 1).first);
+
+ // Max velocity.
+ this->dataPtr->emitter.set_max_velocity(
+ _sdf->Get("max_velocity", 1).first);
+
+ // Color start.
+ ignition::math::Color color = ignition::math::Color::White;
+ if (_sdf->HasElement("color_start"))
+ color = _sdf->Get("color_start");
+ ignition::msgs::Set(this->dataPtr->emitter.mutable_color_start(), color);
+
+ // Color end.
+ color = ignition::math::Color::White;
+ if (_sdf->HasElement("color_end"))
+ color = _sdf->Get("color_end");
+ ignition::msgs::Set(this->dataPtr->emitter.mutable_color_end(), color);
+
+ // Scale rate.
+ this->dataPtr->emitter.set_scale_rate(
+ _sdf->Get("scale_rate", 1).first);
+
+ // Color range image.
+ if (_sdf->HasElement("color_range_image"))
+ {
+ auto modelPath = _ecm.ComponentData(_entity);
+ auto colorRangeImagePath = _sdf->Get("color_range_image");
+ auto path = asFullPath(colorRangeImagePath, modelPath.value());
+
+ common::SystemPaths systemPaths;
+ systemPaths.SetFilePathEnv(kResourcePathEnv);
+ auto absolutePath = systemPaths.FindFile(path);
+
+ this->dataPtr->emitter.set_color_range_image(absolutePath);
+ }
+
+ igndbg << "Loading particle emitter:" << std::endl
+ << this->dataPtr->emitter.DebugString() << std::endl;
+
+ // Create components.
+ SdfEntityCreator sdfEntityCreator(_ecm, _eventMgr);
+ sdfEntityCreator.SetParent(this->dataPtr->emitterEntity, _entity);
+
+ _ecm.CreateComponent(this->dataPtr->emitterEntity,
+ components::Name(this->dataPtr->emitter.name()));
+
+ _ecm.CreateComponent(this->dataPtr->emitterEntity,
+ components::ParticleEmitter(this->dataPtr->emitter));
+
+ _ecm.CreateComponent(this->dataPtr->emitterEntity, components::Pose(pose));
+
+ // Advertise the topic to receive particle emitter commands.
+ const std::string kDefaultTopic =
+ "/model/" + model.Name(_ecm) + "/particle_emitter/" + name;
+ std::string topic = _sdf->Get("topic", kDefaultTopic).first;
+ if (!this->dataPtr->node.Subscribe(
+ topic, &ParticleEmitterPrivate::OnCmd, this->dataPtr.get()))
+ {
+ ignerr << "Error subscribing to topic [" << topic << "]. "
+ << "Particle emitter will not receive updates." << std::endl;
+ return;
+ }
+ igndbg << "Subscribed to " << topic << " for receiving particle emitter "
+ << "updates" << std::endl;
+}
+
+//////////////////////////////////////////////////
+void ParticleEmitter::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
+ ignition::gazebo::EntityComponentManager &_ecm)
+{
+ IGN_PROFILE("ParticleEmitter::PreUpdate");
+
+ std::lock_guard lock(this->dataPtr->mutex);
+
+ if (!this->dataPtr->newDataReceived)
+ return;
+
+ // Nothing left to do if paused.
+ if (_info.paused)
+ return;
+
+ this->dataPtr->newDataReceived = false;
+
+ // Create component.
+ auto emitterComp = _ecm.Component(
+ this->dataPtr->emitterEntity);
+ if (!emitterComp)
+ {
+ _ecm.CreateComponent(
+ this->dataPtr->emitterEntity,
+ components::ParticleEmitterCmd(this->dataPtr->userCmd));
+ }
+ else
+ {
+ emitterComp->Data() = this->dataPtr->userCmd;
+
+ // 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(this->dataPtr->emitterEntity,
+ components::ParticleEmitterCmd::typeId,
+ ComponentState::OneTimeChange);
+ }
+
+ igndbg << "New ParticleEmitterCmd component created" << std::endl;
+}
+
+IGNITION_ADD_PLUGIN(ParticleEmitter,
+ ignition::gazebo::System,
+ ParticleEmitter::ISystemConfigure,
+ ParticleEmitter::ISystemPreUpdate)
+
+IGNITION_ADD_PLUGIN_ALIAS(ParticleEmitter,
+ "ignition::gazebo::systems::ParticleEmitter")
diff --git a/src/systems/particle_emitter/ParticleEmitter.hh b/src/systems/particle_emitter/ParticleEmitter.hh
new file mode 100644
index 0000000000..5b3df5e476
--- /dev/null
+++ b/src/systems/particle_emitter/ParticleEmitter.hh
@@ -0,0 +1,141 @@
+/*
+ * 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_EMITTER_HH_
+#define IGNITION_GAZEBO_SYSTEMS_PARTICLE_EMITTER_HH_
+
+#include
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace systems
+{
+ class ParticleEmitterPrivate;
+
+ /// \brief A system for creating a particle emitter.
+ ///
+ /// System parameters
+ ///
+ /// ``: Unique name for the particle emitter. The name will be
+ /// automatically generated if this parameter is not set.
+ /// ``: Rename the particle emitter if one with the same name
+ /// already exists. Default is false.
+ /// ``: The emitter type (point, box, cylinder, ellipsoid).
+ /// Default value is point.
+ /// ``: The pose of the emitter. Default value is {0, 0, 0, 0, 0, 0}.
+ /// ``: The size of the emitter where the particles are sampled.
+ /// Default value is (1, 1, 1).
+ /// Note that the interpretation of the emitter area varies
+ /// depending on the emmiter type:
+ /// - point: The area is ignored.
+ /// - box: The area is interpreted as width X height X depth.
+ /// - cylinder: The area is interpreted as the bounding box of the
+ /// cilinder. The cylinder is oriented along the
+ /// Z-axis.
+ /// - ellipsoid: The area is interpreted as the bounding box of an
+ /// ellipsoid shaped area, i.e. a sphere or
+ /// squashed-sphere area. The parameters are again
+ /// identical to EM_BOX, except that the dimensions
+ /// describe the widest points along each of the
+ /// axes.
+ /// ``: How many particles per second should be emitted.
+ /// Default value is 10.
+ /// `: The number of seconds the emitter is active. A value of 0
+ /// means infinite duration. Default value is 0.
+ /// ``: This is used to turn on or off particle emission.
+ /// Default value is false.
+ /// ``: Set the particle dimensions (width, height, depth).
+ /// Default value is {1, 1, 1}.
+ /// ``: Set the number of seconds each particle will ’live’ for
+ /// before being destroyed. Default value is 5.
+ /// ``: Sets the material which all particles in the emitter will
+ /// use.
+ /// ``: Sets a minimum velocity for each particle (m/s).
+ /// Default value is 1.
+ /// ``: Sets a maximum velocity for each particle (m/s).
+ /// Default value is 1.
+ /// ``: Sets the starting color for all particle emitted.
+ /// The actual color will be interpolated between this color
+ /// and the one set under .
+ /// Color::White is the default color for the particles
+ /// unless a specific function is used.
+ /// To specify a color, RGB values should be passed in.
+ /// For example, to specify red, a user should enter:
+ /// 1 0 0
+ /// Note that this function overrides the particle colors set
+ /// with .
+ /// ``: Sets the end color for all particle emitted.
+ /// The actual color will be interpolated between this color
+ /// and the one set under .
+ /// Color::White is the default color for the particles
+ /// unless a specific function is used (see color_start for
+ /// more information about defining custom colors with RGB
+ /// values).
+ /// Note that this function overrides the particle colors set
+ /// with .
+ /// ``: Sets the amount by which to scale the particles in both x
+ /// and y direction per second. Default value is 1.
+ /// ``: Sets the path to the color image used as an
+ /// affector. This affector modifies the color of
+ /// particles in flight. The colors are taken from a
+ /// specified image file. The range of color values
+ /// begins from the left side of the image and move to
+ /// the right over the lifetime of the particle,
+ /// therefore only the horizontal dimension of the
+ /// image is used.
+ /// Note that this function overrides the particle
+ /// colors set with and .
+ /// ``: Topic used to update particle emitter properties at runtime.
+ /// The default topic is
+ /// /model//particle_emitter/
+ /// Note that the emitter id and name may not be changed.
+ /// See the examples/worlds/particle_emitter.sdf example world for
+ /// example usage.
+ class IGNITION_GAZEBO_VISIBLE ParticleEmitter
+ : public System,
+ public ISystemConfigure,
+ public ISystemPreUpdate
+ {
+ /// \brief Constructor
+ public: ParticleEmitter();
+
+ // 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 c306ec83bc..7250621ca8 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -35,6 +35,7 @@ set(tests
multicopter.cc
multiple_servers.cc
network_handshake.cc
+ particle_emitter.cc
performer_detector.cc
physics_system.cc
play_pause.cc
diff --git a/test/integration/components.cc b/test/integration/components.cc
index c48b60ddef..28b1053329 100644
--- a/test/integration/components.cc
+++ b/test/integration/components.cc
@@ -1580,3 +1580,24 @@ TEST_F(ComponentsTest, ParticleEmitter)
comp3.Deserialize(istr);
EXPECT_EQ(comp1.Data().id(), comp3.Data().id());
}
+
+//////////////////////////////////////////////////
+TEST_F(ComponentsTest, ParticleEmitterCmd)
+{
+ msgs::ParticleEmitter emitter1;
+ emitter1.set_name("emitter1");
+ emitter1.set_emitting(true);
+
+ // Create components.
+ auto comp1 = components::ParticleEmitterCmd(emitter1);
+
+ // Stream operators.
+ std::ostringstream ostr;
+ comp1.Serialize(ostr);
+
+ std::istringstream istr(ostr.str());
+ components::ParticleEmitter comp3;
+ comp3.Deserialize(istr);
+ EXPECT_EQ(comp1.Data().emitting(), comp3.Data().emitting());
+ EXPECT_EQ(comp1.Data().name(), comp3.Data().name());
+}
diff --git a/test/integration/particle_emitter.cc b/test/integration/particle_emitter.cc
new file mode 100644
index 0000000000..4e26133118
--- /dev/null
+++ b/test/integration/particle_emitter.cc
@@ -0,0 +1,170 @@
+/*
+ * 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 ParticleEmitterTest : 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(ParticleEmitterTest, SDFLoad)
+{
+ bool updateCustomChecked{false};
+ bool updateDefaultChecked{false};
+
+ this->LoadWorld("test/worlds/particle_emitter.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());
+ EXPECT_DOUBLE_EQ(1.0, _emitter->Data().duration());
+ EXPECT_TRUE(_emitter->Data().emitting());
+ EXPECT_EQ(math::Vector3d(3, 3, 3),
+ msgs::Convert(_emitter->Data().particle_size()));
+ EXPECT_DOUBLE_EQ(2.0, _emitter->Data().lifetime());
+ // TODO(anyone) add material check here
+ EXPECT_DOUBLE_EQ(10.0, _emitter->Data().min_velocity());
+ EXPECT_DOUBLE_EQ(20.0, _emitter->Data().max_velocity());
+ 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());
+
+ // color range image is empty because the emitter system
+ // will not be able to find a file that does not exist
+ // TODO(anyone) this should return "/path/to/dummy_image.png"
+ // and let rendering do the findFile instead
+ EXPECT_EQ(std::string(),
+ _emitter->Data().color_range_image());
+ }
+ 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());
+ EXPECT_DOUBLE_EQ(0.0, _emitter->Data().duration());
+ EXPECT_FALSE(_emitter->Data().emitting());
+ EXPECT_EQ(math::Vector3d(1, 1, 1),
+ msgs::Convert(_emitter->Data().particle_size()));
+ EXPECT_DOUBLE_EQ(5.0, _emitter->Data().lifetime());
+ // TODO(anyone) add material check here
+ EXPECT_DOUBLE_EQ(1.0, _emitter->Data().min_velocity());
+ EXPECT_DOUBLE_EQ(1.0, _emitter->Data().max_velocity());
+ 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());
+ EXPECT_EQ("", _emitter->Data().color_range_image());
+ }
+
+ return true;
+ });
+ });
+
+ this->server->AddSystem(testSystem.systemPtr);
+ this->server->Run(true, 1, false);
+
+ EXPECT_TRUE(updateCustomChecked);
+ EXPECT_TRUE(updateDefaultChecked);
+}
+
+/////////////////////////////////////////////////
+/// Main
+int main(int _argc, char **_argv)
+{
+ ::testing::InitGoogleTest(&_argc, _argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/worlds/particle_emitter.sdf b/test/worlds/particle_emitter.sdf
new file mode 100644
index 0000000000..1da40a4919
--- /dev/null
+++ b/test/worlds/particle_emitter.sdf
@@ -0,0 +1,148 @@
+
+
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+
+
+ 3D View
+ false
+ false
+ 0
+
+
+
+
+
+
+ ogre
+ 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
+
+
+
+
+
+
+ smoke_emitter
+ box
+ 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
+
+
+
+
+
+
+
+
+
+
+
+