[WIP] Make NavSat work in actors

include/sdf/NavSat.hh

@@ -133,6 +133,21 @@ namespace sdf
     /// \return True if 'this' != _navsat.
     public: bool operator!=(const NavSat &_navsat) const;
 
+    /// \brief Create and return an SDF element filled with data from this
+    /// imu sensor.
+    /// Note that parameter passing functionality is not captured with this
+    /// function.
+    /// \return SDF element pointer with updated sensor values.
+    public: sdf::ElementPtr ToElement() const;
+
+    /// \brief Create and return an SDF element filled with data from this
+    /// imu sensor.
+    /// Note that parameter passing functionality is not captured with this
+    /// function.
+    /// \param[out] _errors Vector of errors.
+    /// \return SDF element pointer with updated sensor values.
+    public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const;
+
     /// \brief Private data pointer.
     GZ_UTILS_IMPL_PTR(dataPtr)
   };

src/NavSat.cc

@@ -15,6 +15,8 @@
  *
  */
 #include "sdf/NavSat.hh"
+#include "sdf/parser.hh"
+#include "Utils.hh"
 
 using namespace sdf;
 using namespace gz;
@@ -191,3 +193,58 @@ bool NavSat::operator!=(const NavSat &_navsat) const
 {
   return !(*this == _navsat);
 }
+
+
+/////////////////////////////////////////////////
+sdf::ElementPtr NavSat::ToElement() const
+{
+  sdf::Errors errors;
+  auto result = this->ToElement(errors);
+  sdf::throwOrPrintErrors(errors);
+  return result;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr NavSat::ToElement(sdf::Errors &_errors) const
+{
+  sdf::ElementPtr elem(new sdf::Element);
+  sdf::initFile("navSat.sdf", elem);
+
+  sdf::ElementPtr positionSensing =
+    elem->GetElement("position_sensing", _errors);
+
+  sdf::ElementPtr positionSensingHorizontal =
+    positionSensing->GetElement("horizontal", _errors);
+  sdf::ElementPtr positionSensingHorizontalNoiseElem = positionSensingHorizontal->GetElement(
+      "noise", _errors);
+  positionSensingHorizontalNoiseElem->Copy(this->dataPtr->horizontalPositionNoise.ToElement(
+      _errors), _errors);
+
+  sdf::ElementPtr positionSensingVertical =
+    positionSensing->GetElement("vertical", _errors);
+  sdf::ElementPtr positionSensingVerticallNoiseElem = positionSensingVertical->GetElement(
+      "noise", _errors);
+  positionSensingVerticallNoiseElem->Copy(this->dataPtr->verticalPositionNoise.ToElement(
+      _errors), _errors);
+
+
+  sdf::ElementPtr velocitySensing =
+    elem->GetElement("velocity_sensing", _errors);
+
+  sdf::ElementPtr velocitySensingHorizontal =
+    velocitySensing->GetElement("horizontal", _errors);
+  sdf::ElementPtr velocitySensingHorizontalNoiseElem = velocitySensingHorizontal->GetElement(
+      "noise", _errors);
+  velocitySensingHorizontalNoiseElem->Copy(this->dataPtr->horizontalVelocityNoise.ToElement(
+      _errors), _errors);
+
+  sdf::ElementPtr velocitySensingVertical =
+    velocitySensing->GetElement("vertical", _errors);
+  sdf::ElementPtr velocitySensingVerticallNoiseElem = velocitySensingVertical->GetElement(
+      "noise", _errors);
+  velocitySensingVerticallNoiseElem->Copy(this->dataPtr->verticalVelocityNoise.ToElement(
+      _errors), _errors);
+
+
+  return elem;
+}

src/Sensor.cc

@@ -827,6 +827,13 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const
     sdf::ElementPtr magnetometerElem = elem->GetElement("magnetometer");
     magnetometerElem->Copy(this->dataPtr->magnetometer->ToElement());
   }
+  // navsat
+  else if (this->Type() == sdf::SensorType::NAVSAT &&
+      this->dataPtr->navSat)
+  {
+    sdf::ElementPtr navsatElem = elem->GetElement("navsat");
+    navsatElem->Copy(this->dataPtr->navSat->Element());
+  }
   else
   {
     std::stringstream ss;