diff --git a/include/sdf/NavSat.hh b/include/sdf/NavSat.hh index 13341132e..ca232e053 100644 --- a/include/sdf/NavSat.hh +++ b/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) }; diff --git a/src/NavSat.cc b/src/NavSat.cc index 8ab7d7e64..e4c6b284a 100644 --- a/src/NavSat.cc +++ b/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; +} diff --git a/src/Sensor.cc b/src/Sensor.cc index 8f3b12055..8cd813354 100644 --- a/src/Sensor.cc +++ b/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;