JointAxis: update calls to use sdf::Errors output

include/sdf/JointAxis.hh

@@ -229,6 +229,16 @@ namespace sdf
     /// \return SDF element pointer with updated joint values.
     public: sdf::ElementPtr ToElement(unsigned int _index = 0u) const;
 
+    /// \brief Create and return an SDF element filled with data from this
+    /// joint axis.
+    /// Note that parameter passing functionality is not captured with this
+    /// function.
+    /// \param[out] _errors Vector of errors.
+    /// \param[in] _index Index of this joint axis
+    /// \return SDF element pointer with updated joint values.
+    public: sdf::ElementPtr ToElement(sdf::Errors &_errors,
+                                      unsigned int _index = 0u) const;
+
     /// \brief Give 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/JointAxis.cc

@@ -106,10 +106,11 @@ Errors JointAxis::Load(ElementPtr _sdf)
     auto errs = this->SetXyz(_sdf->Get<Vector3d>("xyz",
         this->dataPtr->xyz).first);
     std::copy(errs.begin(), errs.end(), std::back_inserter(errors));
-    auto e = _sdf->GetElement("xyz");
+    auto e = _sdf->GetElement("xyz", errors);
     if (e->HasAttribute("expressed_in"))
     {
-      this->dataPtr->xyzExpressedIn = e->Get<std::string>("expressed_in");
+      this->dataPtr->xyzExpressedIn = e->Get<std::string>(
+          errors, "expressed_in");
     }
   }
   else
@@ -121,35 +122,35 @@ Errors JointAxis::Load(ElementPtr _sdf)
   // Load dynamic values, if present
   if (_sdf->HasElement("dynamics"))
   {
-    sdf::ElementPtr dynElement = _sdf->GetElement("dynamics");
-
-    this->dataPtr->damping = dynElement->Get<double>("damping",
-        this->dataPtr->damping).first;
-    this->dataPtr->friction = dynElement->Get<double>("friction",
-        this->dataPtr->friction).first;
-    this->dataPtr->springReference = dynElement->Get<double>("spring_reference",
-        this->dataPtr->springReference).first;
-    this->dataPtr->springStiffness = dynElement->Get<double>("spring_stiffness",
-        this->dataPtr->springStiffness).first;
+    sdf::ElementPtr dynElement = _sdf->GetElement("dynamics", errors);
+
+    this->dataPtr->damping = dynElement->Get<double>(errors,
+        "damping", this->dataPtr->damping).first;
+    this->dataPtr->friction = dynElement->Get<double>(errors,
+        "friction", this->dataPtr->friction).first;
+    this->dataPtr->springReference = dynElement->Get<double>(errors,
+        "spring_reference", this->dataPtr->springReference).first;
+    this->dataPtr->springStiffness = dynElement->Get<double>(errors,
+        "spring_stiffness", this->dataPtr->springStiffness).first;
   }
 
   // Load limit values
   if (_sdf->HasElement("limit"))
   {
-    sdf::ElementPtr limitElement = _sdf->GetElement("limit");
+    sdf::ElementPtr limitElement = _sdf->GetElement("limit", errors);
 
-    this->dataPtr->lower = limitElement->Get<double>("lower",
+    this->dataPtr->lower = limitElement->Get<double>(errors, "lower",
         this->dataPtr->lower).first;
-    this->dataPtr->upper = limitElement->Get<double>("upper",
+    this->dataPtr->upper = limitElement->Get<double>(errors, "upper",
         this->dataPtr->upper).first;
-    this->dataPtr->effort = limitElement->Get<double>("effort",
+    this->dataPtr->effort = limitElement->Get<double>(errors, "effort",
         this->dataPtr->effort).first;
-    this->dataPtr->maxVelocity = limitElement->Get<double>("velocity",
+    this->dataPtr->maxVelocity = limitElement->Get<double>(errors, "velocity",
         this->dataPtr->maxVelocity).first;
-    this->dataPtr->stiffness = limitElement->Get<double>("stiffness",
+    this->dataPtr->stiffness = limitElement->Get<double>(errors, "stiffness",
         this->dataPtr->stiffness).first;
-    this->dataPtr->dissipation = limitElement->Get<double>("dissipation",
-        this->dataPtr->dissipation).first;
+    this->dataPtr->dissipation = limitElement->Get<double>(errors,
+        "dissipation", this->dataPtr->dissipation).first;
   }
   else
   {
@@ -376,35 +377,53 @@ sdf::ElementPtr JointAxis::Element() const
 
 /////////////////////////////////////////////////
 sdf::ElementPtr JointAxis::ToElement(unsigned int _index) const
+{
+  sdf::Errors errors;
+  auto result = this->ToElement(errors, _index);
+  sdf::throwOrPrintErrors(errors);
+  return result;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr JointAxis::ToElement(sdf::Errors &_errors,
+                                     unsigned int _index) const
 {
   sdf::ElementPtr elem(new sdf::Element);
   sdf::initFile("joint.sdf", elem);
 
   std::string axisElemName = "axis";
   if (_index > 0u)
     axisElemName += std::to_string(_index + 1);
-  sdf::ElementPtr axisElem = elem->GetElement(axisElemName);
-  sdf::ElementPtr xyzElem = axisElem->GetElement("xyz");
-  xyzElem->Set<gz::math::Vector3d>(this->Xyz());
+  sdf::ElementPtr axisElem = elem->GetElement(axisElemName, _errors);
+  sdf::ElementPtr xyzElem = axisElem->GetElement("xyz", _errors);
+  xyzElem->Set<gz::math::Vector3d>(_errors, this->Xyz());
   if (!this->XyzExpressedIn().empty())
   {
     xyzElem->GetAttribute("expressed_in")->Set<std::string>(
-        this->XyzExpressedIn());
+        this->XyzExpressedIn(), _errors);
   }
-  sdf::ElementPtr dynElem = axisElem->GetElement("dynamics");
-  dynElem->GetElement("damping")->Set<double>(this->Damping());
-  dynElem->GetElement("friction")->Set<double>(this->Friction());
-  dynElem->GetElement("spring_reference")->Set<double>(
-      this->SpringReference());
-  dynElem->GetElement("spring_stiffness")->Set<double>(
-      this->SpringStiffness());
-
-  sdf::ElementPtr limitElem = axisElem->GetElement("limit");
-  limitElem->GetElement("lower")->Set<double>(this->Lower());
-  limitElem->GetElement("upper")->Set<double>(this->Upper());
-  limitElem->GetElement("effort")->Set<double>(this->Effort());
-  limitElem->GetElement("velocity")->Set<double>(this->MaxVelocity());
-  limitElem->GetElement("stiffness")->Set<double>(this->Stiffness());
-  limitElem->GetElement("dissipation")->Set<double>(this->Dissipation());
+  sdf::ElementPtr dynElem = axisElem->GetElement("dynamics", _errors);
+  dynElem->GetElement("damping", _errors)->Set<double>(
+      _errors, this->Damping());
+  dynElem->GetElement("friction", _errors)->Set<double>(
+      _errors, this->Friction());
+  dynElem->GetElement("spring_reference", _errors)->Set<double>(
+      _errors, this->SpringReference());
+  dynElem->GetElement("spring_stiffness", _errors)->Set<double>(
+      _errors, this->SpringStiffness());
+
+  sdf::ElementPtr limitElem = axisElem->GetElement("limit", _errors);
+  limitElem->GetElement("lower", _errors)->Set<double>(
+      _errors, this->Lower());
+  limitElem->GetElement("upper", _errors)->Set<double>(
+      _errors, this->Upper());
+  limitElem->GetElement("effort", _errors)->Set<double>(
+      _errors, this->Effort());
+  limitElem->GetElement("velocity", _errors)->Set<double>(
+      _errors, this->MaxVelocity());
+  limitElem->GetElement("stiffness", _errors)->Set<double>(
+      _errors, this->Stiffness());
+  limitElem->GetElement("dissipation", _errors)->Set<double>(
+      _errors, this->Dissipation());
   return axisElem;
 }

src/JointAxis_TEST.cc

@@ -18,6 +18,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include "sdf/JointAxis.hh"
+#include "test_utils.hh"
 
 /////////////////////////////////////////////////
 TEST(DOMJointAxis, Construction)
@@ -156,3 +157,113 @@ TEST(DOMJointAxis, ZeroNormVectorReturnsError)
   ASSERT_FALSE(errors.empty());
   EXPECT_EQ(errors[0].Message(), "The norm of the xyz vector cannot be zero");
 }
+
+/////////////////////////////////////////////////
+TEST(DOMJointAxis, ToElement)
+{
+  std::stringstream buffer;
+  sdf::testing::RedirectConsoleStream redir(
+      sdf::Console::Instance()->GetMsgStream(), &buffer);
+
+  #ifdef _WIN32
+    sdf::Console::Instance()->SetQuiet(false);
+    sdf::testing::ScopeExit revertSetQuiet(
+      []
+      {
+        sdf::Console::Instance()->SetQuiet(true);
+      });
+  #endif
+
+  sdf::JointAxis axis;
+  sdf::Errors errors;
+
+  errors = axis.SetXyz(gz::math::Vector3d(0, 1, 0));
+  ASSERT_TRUE(errors.empty());
+  axis.SetXyzExpressedIn("test");
+  ASSERT_TRUE(errors.empty());
+
+  axis.SetDamping(0.2);
+  axis.SetFriction(1.3);
+  axis.SetSpringReference(2.4);
+  axis.SetSpringStiffness(-1.2);
+  axis.SetLower(-10.8);
+  axis.SetUpper(123.4);
+  axis.SetEffort(3.2);
+  axis.SetMaxVelocity(54.2);
+  axis.SetStiffness(1e2);
+  axis.SetDissipation(1.5);
+
+  sdf::ElementPtr elem = axis.ToElement(errors);
+  ASSERT_TRUE(errors.empty());
+
+  sdf::ElementPtr xyzElem = elem->GetElement("xyz", errors);
+  ASSERT_TRUE(errors.empty());
+  gz::math::Vector3d xyz = elem->Get<gz::math::Vector3d>(
+      errors, "xyz", xyz).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_EQ(gz::math::Vector3d::UnitY, axis.Xyz());
+  std::string expressedIn = elem->GetElement("xyz", errors)->Get<std::string>(
+      errors, "expressed_in");
+  ASSERT_TRUE(errors.empty());
+  EXPECT_EQ("test", expressedIn);
+
+  sdf::ElementPtr dynElem = elem->GetElement("dynamics", errors);
+  ASSERT_TRUE(errors.empty());
+
+  double damping;
+  damping = dynElem->Get<double>(errors, "damping", damping).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(0.2, damping);
+
+  double friction;
+  friction = dynElem->Get<double>(errors, "friction", friction).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(1.3, friction);
+
+  double springReference;
+  springReference = dynElem->Get<double>(
+      errors, "spring_reference", springReference).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(2.4, springReference);
+
+  double springStiffness;
+  springStiffness = dynElem->Get<double>(
+      errors, "spring_stiffness", springStiffness).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(-1.2, springStiffness);
+
+  sdf::ElementPtr limitElem = elem->GetElement("limit", errors);
+  double lower;
+  lower = limitElem->Get<double>(errors, "lower", lower).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(-10.8, lower);
+
+  double upper;
+  upper = limitElem->Get<double>(errors, "upper", upper).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(123.4, upper);
+
+  double effort;
+  effort = limitElem->Get<double>(errors, "effort", effort).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(3.2, effort);
+
+  double maxVel;
+  maxVel = limitElem->Get<double>(errors, "velocity", maxVel).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(54.2, maxVel);
+
+  double stiffness;
+  stiffness = limitElem->Get<double>(errors, "stiffness", stiffness).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(1e2, stiffness);
+
+  double dissipation;
+  dissipation = limitElem->Get<double>(
+      errors, "dissipation", dissipation).first;
+  ASSERT_TRUE(errors.empty());
+  EXPECT_DOUBLE_EQ(1.5, dissipation);
+
+  // Check nothing has been printed
+  EXPECT_TRUE(buffer.str().empty()) << buffer.str();
+}