Added ToElement for ParticleEmitter and Link

include/sdf/Link.hh

@@ -276,6 +276,12 @@ namespace sdf
     /// exists.
     public: bool AddSensor(const Sensor &_sensor);
 
+    /// \brief Add a particle emitter to the link.
+    /// \param[in] _emitter Particle emitter to add.
+    /// \return True if successful, false if a particle emitter with the name
+    /// already exists.
+    public: bool AddParticleEmitter(const ParticleEmitter &_sensor);
+
     /// \brief Remove all collisions
     public: void ClearCollisions();
 
@@ -288,6 +294,14 @@ namespace sdf
     /// \brief Remove all sensors
     public: void ClearSensors();
 
+    /// \brief Remove all particle emitters
+    public: void ClearParticleEmitters();
+
+    /// \brief Create and return an SDF element filled with data from this
+    /// link.
+    /// \return SDF element pointer with updated link values.
+    public: sdf::ElementPtr ToElement() const;
+
     /// \brief Private data pointer.
     IGN_UTILS_IMPL_PTR(dataPtr)
   };

include/sdf/ParticleEmitter.hh

@@ -316,6 +316,11 @@ namespace sdf
     /// \paramp[in] _filePath Full path to the file on disk.
     public: void SetFilePath(const std::string &_filePath);
 
+    /// \brief Create and return an SDF element filled with data from this
+    /// particle emitter.
+    /// \return SDF element pointer with updated particle emitter values.
+    public: sdf::ElementPtr ToElement() const;
+
     /// \brief Set the name of the xml parent of this object, to be used
     /// for resolving poses. This is private and is intended to be called by
     /// Link::SetPoseRelativeToGraph.

src/Link.cc

@@ -25,6 +25,7 @@
 #include "sdf/Error.hh"
 #include "sdf/Light.hh"
 #include "sdf/Link.hh"
+#include "sdf/parser.hh"
 #include "sdf/ParticleEmitter.hh"
 #include "sdf/Sensor.hh"
 #include "sdf/Types.hh"
@@ -534,6 +535,15 @@ bool Link::AddSensor(const Sensor &_sensor)
   return true;
 }
 
+//////////////////////////////////////////////////
+bool Link::AddParticleEmitter(const ParticleEmitter &_emitter)
+{
+  if (this->ParticleEmitterNameExists(_emitter.Name()))
+    return false;
+  this->dataPtr->emitters.push_back(_emitter);
+  return true;
+}
+
 //////////////////////////////////////////////////
 void Link::ClearCollisions()
 {
@@ -557,3 +567,77 @@ void Link::ClearSensors()
 {
   this->dataPtr->sensors.clear();
 }
+
+//////////////////////////////////////////////////
+void Link::ClearParticleEmitters()
+{
+  this->dataPtr->emitters.clear();
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Link::ToElement() const
+{
+  sdf::ElementPtr elem(new sdf::Element);
+  sdf::initFile("link.sdf", elem);
+
+  elem->GetAttribute("name")->Set(this->Name());
+
+  // Set pose
+  sdf::ElementPtr poseElem = elem->GetElement("pose");
+  if (!this->dataPtr->poseRelativeTo.empty())
+  {
+    poseElem->GetAttribute("relative_to")->Set<std::string>(
+        this->dataPtr->poseRelativeTo);
+  }
+  poseElem->Set<ignition::math::Pose3d>(this->RawPose());
+
+  // inertial
+  sdf::ElementPtr inertialElem = elem->GetElement("inertial");
+  inertialElem->GetElement("pose")->Set(
+      this->dataPtr->inertial.Pose());
+  const ignition::math::MassMatrix3d &massMatrix =
+    this->dataPtr->inertial.MassMatrix();
+  inertialElem->GetElement("mass")->Set<double>(massMatrix.Mass());
+  sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
+  inertiaElem->GetElement("ixx")->Set(massMatrix.Ixx());
+  inertiaElem->GetElement("ixy")->Set(massMatrix.Ixy());
+  inertiaElem->GetElement("ixz")->Set(massMatrix.Ixz());
+  inertiaElem->GetElement("iyy")->Set(massMatrix.Iyy());
+  inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz());
+  inertiaElem->GetElement("izz")->Set(massMatrix.Izz());
+
+  // wind mode
+  elem->GetElement("enable_wind")->Set(this->EnableWind());
+
+  // Collisions
+  for (const sdf::Collision &collision : this->dataPtr->collisions)
+  {
+    elem->InsertElement(collision.ToElement());
+  }
+
+  // Light
+  for (const sdf::Light &light : this->dataPtr->lights)
+  {
+    elem->InsertElement(light.ToElement());
+  }
+
+  // Particle emitters
+  for (const sdf::ParticleEmitter &emitter : this->dataPtr->emitters)
+  {
+    elem->InsertElement(emitter.ToElement());
+  }
+
+  // Sensors
+  for (const sdf::Sensor &sensor : this->dataPtr->sensors)
+  {
+    elem->InsertElement(sensor.ToElement());
+  }
+
+  // Visuals
+  for (const sdf::Visual &visual : this->dataPtr->visuals)
+  {
+    elem->InsertElement(visual.ToElement());
+  }
+
+  return elem;
+}

src/Link_TEST.cc

@@ -303,3 +303,109 @@ TEST(DOMLink, AddSensor)
   ASSERT_NE(nullptr, sensorFromLink);
   EXPECT_EQ(sensorFromLink->Name(), sensor.Name());
 }
+
+/////////////////////////////////////////////////
+TEST(DOMLink, ToElement)
+{
+  sdf::Link link;
+  link.SetName("my-name");
+
+  ignition::math::Inertiald inertial {
+    {2.3,
+      ignition::math::Vector3d(1.4, 2.3, 3.2),
+      ignition::math::Vector3d(0.1, 0.2, 0.3)},
+      ignition::math::Pose3d(1, 2, 3, 0, 0, 0)};
+  EXPECT_TRUE(link.SetInertial(inertial));
+  link.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
+  link.SetEnableWind(true);
+
+  for (int j = 0; j < 1; ++j)
+  {
+    for (int i = 0; i < 1; ++i)
+    {
+      sdf::Collision collision;
+      collision.SetName("collision" + std::to_string(i));
+      EXPECT_TRUE(link.AddCollision(collision));
+      EXPECT_FALSE(link.AddCollision(collision));
+    }
+    link.ClearCollisions();
+  }
+
+  for (int j = 0; j < 1; ++j)
+  {
+   for (int i = 0; i < 2; i++)
+   {
+     sdf::Visual visual;
+     visual.SetName("visual" + std::to_string(i));
+     EXPECT_TRUE(link.AddVisual(visual));
+     EXPECT_FALSE(link.AddVisual(visual));
+   }
+   link.ClearVisuals();
+  }
+
+  for (int j = 0; j < 1; ++j)
+  {
+    for (int i = 0; i < 3; i++)
+    {
+      sdf::Light light;
+      light.SetName("light" + std::to_string(i));
+      EXPECT_TRUE(link.AddLight(light));
+      EXPECT_FALSE(link.AddLight(light));
+    }
+    link.ClearLights();
+  }
+
+  for (int j = 0; j < 1; ++j)
+  {
+    for (int i = 0; i < 4; i++)
+    {
+      sdf::Sensor sensor;
+      sensor.SetName("sensor" + std::to_string(i));
+      EXPECT_TRUE(link.AddSensor(sensor));
+      EXPECT_FALSE(link.AddSensor(sensor));
+    }
+    link.ClearSensors();
+  }
+
+  for (int j = 0; j < 1; ++j)
+  {
+    for (int i = 0; i < 5; i++)
+    {
+      sdf::ParticleEmitter emitter;
+      emitter.SetName("emitter" + std::to_string(i));
+      EXPECT_TRUE(link.AddParticleEmitter(emitter));
+      EXPECT_FALSE(link.AddParticleEmitter(emitter));
+    }
+    link.ClearParticleEmitters();
+  }
+
+  sdf::ElementPtr elem = link.ToElement();
+  ASSERT_NE(nullptr, elem);
+
+  sdf::Link link2;
+  link2.Load(elem);
+
+  EXPECT_EQ(link.Name(), link2.Name());
+  EXPECT_EQ(link.Inertial(), link2.Inertial());
+  EXPECT_EQ(link.RawPose(), link2.RawPose());
+  EXPECT_EQ(link.EnableWind(), link2.EnableWind());
+  EXPECT_EQ(link.CollisionCount(), link2.CollisionCount());
+  for (uint64_t i = 0; i < link2.CollisionCount(); ++i)
+    EXPECT_NE(nullptr, link2.CollisionByIndex(i));
+
+  EXPECT_EQ(link.VisualCount(), link2.VisualCount());
+  for (uint64_t i = 0; i < link2.VisualCount(); ++i)
+    EXPECT_NE(nullptr, link2.VisualByIndex(i));
+
+  EXPECT_EQ(link.LightCount(), link2.LightCount());
+  for (uint64_t i = 0; i < link2.LightCount(); ++i)
+    EXPECT_NE(nullptr, link2.LightByIndex(i));
+
+  EXPECT_EQ(link.SensorCount(), link2.SensorCount());
+  for (uint64_t i = 0; i < link2.SensorCount(); ++i)
+    EXPECT_NE(nullptr, link2.SensorByIndex(i));
+
+  EXPECT_EQ(link.ParticleEmitterCount(), link2.ParticleEmitterCount());
+  for (uint64_t i = 0; i < link2.ParticleEmitterCount(); ++i)
+    EXPECT_NE(nullptr, link2.ParticleEmitterByIndex(i));
+}

src/ParticleEmitter.cc

@@ -19,8 +19,9 @@
 #include <string>
 #include <vector>
 
-#include "sdf/ParticleEmitter.hh"
 #include "sdf/Error.hh"
+#include "sdf/parser.hh"
+#include "sdf/ParticleEmitter.hh"
 #include "sdf/Types.hh"
 
 #include "FrameSemantics.hh"
@@ -515,3 +516,45 @@ void ParticleEmitter::SetPoseRelativeToGraph(
 {
   this->dataPtr->poseRelativeToGraph = _graph;
 }
+
+/////////////////////////////////////////////////
+sdf::ElementPtr ParticleEmitter::ToElement() const
+{
+  sdf::ElementPtr elem(new sdf::Element);
+  sdf::initFile("particle_emitter.sdf", elem);
+
+  // Set pose
+  sdf::ElementPtr poseElem = elem->GetElement("pose");
+  if (!this->dataPtr->poseRelativeTo.empty())
+  {
+    poseElem->GetAttribute("relative_to")->Set<std::string>(
+        this->dataPtr->poseRelativeTo);
+  }
+  poseElem->Set<ignition::math::Pose3d>(this->RawPose());
+
+  elem->GetAttribute("name")->Set(this->Name());
+  elem->GetAttribute("type")->Set(this->TypeStr());
+  elem->GetElement("emitting")->Set(this->Emitting());
+  elem->GetElement("duration")->Set(this->Duration());
+  elem->GetElement("size")->Set(this->Size());
+  elem->GetElement("particle_size")->Set(this->ParticleSize());
+  elem->GetElement("lifetime")->Set(this->Lifetime());
+  elem->GetElement("rate")->Set(this->Rate());
+  elem->GetElement("min_velocity")->Set(this->MinVelocity());
+  elem->GetElement("max_velocity")->Set(this->MaxVelocity());
+  elem->GetElement("scale_rate")->Set(this->ScaleRate());
+  elem->GetElement("color_start")->Set(this->ColorStart());
+  elem->GetElement("color_end")->Set(this->ColorEnd());
+  elem->GetElement("color_range_image")->Set(this->ColorRangeImage());
+  elem->GetElement("topic")->Set(this->Topic());
+  elem->GetElement("particle_scatter_ratio")->Set(this->ScatterRatio());
+
+  if (this->dataPtr->material)
+  {
+    elem->InsertElement(this->dataPtr->material->ToElement());
+  }
+
+  return elem;
+}
+
+

src/ParticleEmitter_TEST.cc

@@ -116,3 +116,54 @@ TEST(DOMParticleEmitter, Construction)
   emitter.SetPoseRelativeTo("/test/relative");
   EXPECT_EQ("/test/relative", emitter.PoseRelativeTo());
 }
+
+/////////////////////////////////////////////////
+TEST(DOMParticleEmitter, ToElement)
+{
+  sdf::ParticleEmitter emitter;
+
+  emitter.SetName("my-emitter");
+  emitter.SetType(sdf::ParticleEmitterType::BOX);
+  emitter.SetEmitting(true);
+  emitter.SetDuration(1.2);
+  emitter.SetLifetime(3.4);
+  emitter.SetRate(12.5);
+  emitter.SetScaleRate(0.2);
+  emitter.SetMinVelocity(32.4);
+  emitter.SetMaxVelocity(50.1);
+  emitter.SetSize(ignition::math::Vector3d(1, 2, 3));
+  emitter.SetParticleSize(ignition::math::Vector3d(4, 5, 6));
+  emitter.SetColorStart(ignition::math::Color(0.1f, 0.2f, 0.3f, 1.0f));
+  emitter.SetColorEnd(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0f));
+  emitter.SetColorRangeImage("my-image");
+  emitter.SetTopic("my-topic");
+  emitter.SetScatterRatio(0.3f);
+  emitter.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
+  sdf::Material material;
+  emitter.SetMaterial(material);
+
+  sdf::ElementPtr elem = emitter.ToElement();
+  ASSERT_NE(nullptr, elem);
+
+  sdf::ParticleEmitter emitter2;
+  emitter2.Load(elem);
+
+  EXPECT_EQ(emitter.Name(), emitter2.Name());
+  EXPECT_EQ(emitter.Type(), emitter2.Type());
+  EXPECT_EQ(emitter.Emitting(), emitter2.Emitting());
+  EXPECT_DOUBLE_EQ(emitter.Duration(), emitter2.Duration());
+  EXPECT_DOUBLE_EQ(emitter.Lifetime(), emitter2.Lifetime());
+  EXPECT_DOUBLE_EQ(emitter.Rate(), emitter2.Rate());
+  EXPECT_DOUBLE_EQ(emitter.ScaleRate(), emitter2.ScaleRate());
+  EXPECT_DOUBLE_EQ(emitter.MinVelocity(), emitter2.MinVelocity());
+  EXPECT_DOUBLE_EQ(emitter.MaxVelocity(), emitter2.MaxVelocity());
+  EXPECT_EQ(emitter.Size(), emitter2.Size());
+  EXPECT_EQ(emitter.ParticleSize(), emitter2.ParticleSize());
+  EXPECT_EQ(emitter.ColorStart(), emitter2.ColorStart());
+  EXPECT_EQ(emitter.ColorEnd(), emitter2.ColorEnd());
+  EXPECT_EQ(emitter.ColorRangeImage(), emitter2.ColorRangeImage());
+  EXPECT_EQ(emitter.Topic(), emitter2.Topic());
+  EXPECT_FLOAT_EQ(emitter.ScatterRatio(), emitter2.ScatterRatio());
+  EXPECT_EQ(emitter.RawPose(), emitter2.RawPose());
+  EXPECT_NE(nullptr, emitter2.Material());
+}