NavSat::ToElement() method

include/sdf/Error.hh

@@ -46,7 +46,7 @@ namespace sdf
   /// \sa Errors
   enum class ErrorCode
   {
-    // \brief No error
+    /// \brief No error
     NONE = 0,
 
     /// \brief Indicates that reading an SDF file failed.

include/sdf/NavSat.hh

@@ -133,6 +133,13 @@ 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
+    /// NavSat.
+    /// 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 Private data pointer.
     GZ_UTILS_IMPL_PTR(dataPtr)
   };

src/Geometry.cc

@@ -41,7 +41,7 @@ using namespace sdf;
 // Private data class
 class sdf::Geometry::Implementation
 {
-  // \brief The geometry type.
+  /// \brief The geometry type.
   public: GeometryType type = GeometryType::EMPTY;
 
   /// \brief Optional box.

src/NavSat.cc

@@ -15,6 +15,7 @@
  *
  */
 #include "sdf/NavSat.hh"
+#include "sdf/parser.hh"
 
 using namespace sdf;
 using namespace gz;
@@ -191,3 +192,41 @@ bool NavSat::operator!=(const NavSat &_navsat) const
 {
   return !(*this == _navsat);
 }
+
+/////////////////////////////////////////////////
+sdf::ElementPtr NavSat::ToElement() const
+{
+  sdf::ElementPtr elem(new sdf::Element);
+  sdf::initFile("navsat.sdf", elem);
+  const auto defaultNoise = sdf::Noise();
+
+  if (this->dataPtr->horizontalPositionNoise != defaultNoise)
+  {
+    auto el = elem->GetElement("position_sensing")->
+        GetElement("horizontal")->GetElement("noise");
+    el->Copy(this->dataPtr->horizontalPositionNoise.ToElement());
+  }
+
+  if (this->dataPtr->verticalPositionNoise != defaultNoise)
+  {
+    auto el = elem->GetElement("position_sensing")->
+        GetElement("vertical")->GetElement("noise");
+    el->Copy(this->dataPtr->verticalPositionNoise.ToElement());
+  }
+
+  if (this->dataPtr->horizontalVelocityNoise != defaultNoise)
+  {
+    auto el = elem->GetElement("velocity_sensing")->
+        GetElement("horizontal")->GetElement("noise");
+    el->Copy(this->dataPtr->horizontalVelocityNoise.ToElement());
+  }
+
+  if (this->dataPtr->verticalVelocityNoise != defaultNoise)
+  {
+    auto el = elem->GetElement("velocity_sensing")->
+        GetElement("vertical")->GetElement("noise");
+    el->Copy(this->dataPtr->verticalVelocityNoise.ToElement());
+  }
+
+  return elem;
+}

src/NavSat_TEST.cc

@@ -113,3 +113,43 @@ TEST(DOMNavSat, Load)
   EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code());
   EXPECT_NE(nullptr, navSat.Element());
 }
+
+/////////////////////////////////////////////////
+TEST(DOMNavSat, ToElement)
+{
+  sdf::NavSat navSat;
+  sdf::Noise noiseHorPos;
+  noiseHorPos.SetType(sdf::NoiseType::GAUSSIAN);
+  noiseHorPos.SetMean(1.2);
+  noiseHorPos.SetStdDev(2.3);
+  noiseHorPos.SetBiasMean(4.5);
+  noiseHorPos.SetBiasStdDev(6.7);
+  noiseHorPos.SetPrecision(8.9);
+  navSat.SetHorizontalPositionNoise(noiseHorPos);
+
+  auto noiseVerPos = noiseHorPos;
+  noiseVerPos.SetMean(2.2);
+  navSat.SetVerticalPositionNoise(noiseVerPos);
+
+  auto noiseHorVel = noiseHorPos;
+  noiseHorVel.SetMean(3.2);
+  navSat.SetHorizontalVelocityNoise(noiseHorVel);
+
+  auto noiseVerVel = noiseHorPos;
+  noiseVerVel.SetMean(4.2);
+  navSat.SetVerticalVelocityNoise(noiseVerVel);
+
+  sdf::ElementPtr navSatElem = navSat.ToElement();
+  ASSERT_NE(nullptr, navSatElem);
+  EXPECT_EQ(nullptr, navSat.Element());
+
+  // verify values after loading the element back
+  sdf::NavSat navSat2;
+  auto errors = navSat2.Load(navSatElem);
+  ASSERT_TRUE(errors.empty());
+
+  EXPECT_EQ(noiseHorPos, navSat2.HorizontalPositionNoise());
+  EXPECT_EQ(noiseVerPos, navSat2.VerticalPositionNoise());
+  EXPECT_EQ(noiseHorVel, navSat2.HorizontalVelocityNoise());
+  EXPECT_EQ(noiseVerVel, navSat2.VerticalVelocityNoise());
+}

src/ScopedGraph.hh

@@ -139,9 +139,9 @@ class ScopedGraph
   /// \return A new child scope.
   public: ScopedGraph<T> ChildModelScope(const std::string &_name) const;
 
-  // \brief Get the root scope (the scope without any prefix). This is useful
-  // for resolving poses relative to the world.
-  // \return A new scope anchored at the root of the graph
+  /// \brief Get the root scope (the scope without any prefix). This is useful
+  /// for resolving poses relative to the world.
+  /// \return A new scope anchored at the root of the graph
   public: ScopedGraph<T> RootScope() const;
 
   /// \brief Checks if the scope points to a valid graph.

src/Sensor.cc

@@ -76,7 +76,7 @@ constexpr std::array<const std::string_view, 27> kSensorTypeStrs =
 
 class sdf::Sensor::Implementation
 {
-  // \brief The sensor type.
+  /// \brief The sensor type.
   public: SensorType type = SensorType::NONE;
 
   /// \brief Name of the sensor.
@@ -772,25 +772,31 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const
   elem->GetElement("enable_metrics")->Set<double>(this->EnableMetrics());
 
   // air pressure
-  if (this->Type() == sdf::SensorType::AIR_PRESSURE &&
-      this->dataPtr->airPressure)
+  if (this->Type() == sdf::SensorType::AIR_PRESSURE)
   {
-    sdf::ElementPtr airPressureElem = elem->GetElement("air_pressure");
-    airPressureElem->Copy(this->dataPtr->airPressure->ToElement());
+    if (this->dataPtr->airPressure)
+    {
+      sdf::ElementPtr airPressureElem = elem->GetElement("air_pressure");
+      airPressureElem->Copy(this->dataPtr->airPressure->ToElement());
+    }
   }
   // air speed
-  else if (this->Type() == sdf::SensorType::AIR_SPEED &&
-      this->dataPtr->airSpeed)
+  else if (this->Type() == sdf::SensorType::AIR_SPEED)
   {
-    sdf::ElementPtr airSpeedElem = elem->GetElement("air_speed");
-    airSpeedElem->Copy(this->dataPtr->airSpeed->ToElement());
+    if (this->dataPtr->airSpeed)
+    {
+      sdf::ElementPtr airSpeedElem = elem->GetElement("air_speed");
+      airSpeedElem->Copy(this->dataPtr->airSpeed->ToElement());
+    }
   }
   // altimeter
-  else if (this->Type() == sdf::SensorType::ALTIMETER &&
-      this->dataPtr->altimeter)
+  else if (this->Type() == sdf::SensorType::ALTIMETER)
   {
-    sdf::ElementPtr altimeterElem = elem->GetElement("altimeter");
-    altimeterElem->Copy(this->dataPtr->altimeter->ToElement());
+    if (this->dataPtr->altimeter)
+    {
+      sdf::ElementPtr altimeterElem = elem->GetElement("altimeter");
+      altimeterElem->Copy(this->dataPtr->altimeter->ToElement());
+    }
   }
   // camera, depth, thermal, segmentation
   else if (this->CameraSensor())
@@ -799,39 +805,56 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const
     cameraElem->Copy(this->dataPtr->camera->ToElement());
   }
   // force torque
-  else if (this->Type() == sdf::SensorType::FORCE_TORQUE  &&
-      this->dataPtr->forceTorque)
+  else if (this->Type() == sdf::SensorType::FORCE_TORQUE)
   {
-    sdf::ElementPtr forceTorqueElem = elem->GetElement("force_torque");
-    forceTorqueElem->Copy(this->dataPtr->forceTorque->ToElement());
+    if (this->dataPtr->forceTorque)
+    {
+      sdf::ElementPtr forceTorqueElem = elem->GetElement("force_torque");
+      forceTorqueElem->Copy(this->dataPtr->forceTorque->ToElement());
+    }
   }
   // imu
-  else if (this->Type() == sdf::SensorType::IMU && this->dataPtr->imu)
+  else if (this->Type() == sdf::SensorType::IMU)
   {
-    sdf::ElementPtr imuElem = elem->GetElement("imu");
-    imuElem->Copy(this->dataPtr->imu->ToElement());
+    if (this->dataPtr->imu)
+    {
+      sdf::ElementPtr imuElem = elem->GetElement("imu");
+      imuElem->Copy(this->dataPtr->imu->ToElement());
+    }
   }
   // lidar, gpu_lidar
-  else if ((this->Type() == sdf::SensorType::GPU_LIDAR ||
-            this->Type() == sdf::SensorType::LIDAR) &&
-           this->dataPtr->lidar)
+  else if (this->Type() == sdf::SensorType::GPU_LIDAR ||
+           this->Type() == sdf::SensorType::LIDAR)
   {
-    sdf::ElementPtr rayElem = (elem->HasElement("ray")) ?
-        elem->GetElement("ray") : elem->GetElement("lidar");
-    rayElem->Copy(this->dataPtr->lidar->ToElement());
+    if (this->dataPtr->lidar)
+    {
+      sdf::ElementPtr rayElem = (elem->HasElement("ray")) ?
+          elem->GetElement("ray") : elem->GetElement("lidar");
+      rayElem->Copy(this->dataPtr->lidar->ToElement());
+    }
   }
   // magnetometer
-  else if (this->Type() == sdf::SensorType::MAGNETOMETER &&
-      this->dataPtr->magnetometer)
+  else if (this->Type() == sdf::SensorType::MAGNETOMETER)
   {
-    sdf::ElementPtr magnetometerElem = elem->GetElement("magnetometer");
-    magnetometerElem->Copy(this->dataPtr->magnetometer->ToElement());
+    if (this->dataPtr->magnetometer)
+    {
+      sdf::ElementPtr magnetometerElem = elem->GetElement("magnetometer");
+      magnetometerElem->Copy(this->dataPtr->magnetometer->ToElement());
+    }
+  }
+  else if (this->Type() == sdf::SensorType::NAVSAT)
+  {
+    if (this->dataPtr->navSat)
+    {
+      sdf::ElementPtr navSatElem = elem->GetElement("navsat");
+      navSatElem->Copy(this->dataPtr->navSat->ToElement());
+    }
   }
   else
   {
     std::stringstream ss;
     ss << "Conversion of sensor type: [" << this->TypeStr() << "] from SDF "
-       << "DOM to Element is not supported yet." << this->Name();
+       "DOM to Element is not supported yet." << this->Name() << '\n';
     _errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
   }
 

src/Surface.cc

@@ -30,7 +30,7 @@ using namespace sdf;
 
 class sdf::Contact::Implementation
 {
-  // \brief The bitmask used to filter collisions.
+  /// \brief The bitmask used to filter collisions.
   public: uint16_t collideBitmask = 0xff;
 
   /// \brief The SDF element pointer used during load.