From 64748a310e002c632b4ecf873153aeaab3d40d59 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Tue, 28 Mar 2023 13:59:21 +0800 Subject: [PATCH 1/6] JointAxis: update calls to use sdf::Errors output (#1145) Signed-off-by: Marco A. Gutierrez --- include/sdf/JointAxis.hh | 10 ++++ src/JointAxis.cc | 97 ++++++++++++++++++++-------------- src/JointAxis_TEST.cc | 111 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 179 insertions(+), 39 deletions(-) diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index deac2965b..768bcd989 100644 --- a/include/sdf/JointAxis.hh +++ b/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. diff --git a/src/JointAxis.cc b/src/JointAxis.cc index 735254ea1..39eb5ea50 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -106,10 +106,11 @@ Errors JointAxis::Load(ElementPtr _sdf) auto errs = this->SetXyz(_sdf->Get("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("expressed_in"); + this->dataPtr->xyzExpressedIn = e->Get( + 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("damping", - this->dataPtr->damping).first; - this->dataPtr->friction = dynElement->Get("friction", - this->dataPtr->friction).first; - this->dataPtr->springReference = dynElement->Get("spring_reference", - this->dataPtr->springReference).first; - this->dataPtr->springStiffness = dynElement->Get("spring_stiffness", - this->dataPtr->springStiffness).first; + sdf::ElementPtr dynElement = _sdf->GetElement("dynamics", errors); + + this->dataPtr->damping = dynElement->Get(errors, + "damping", this->dataPtr->damping).first; + this->dataPtr->friction = dynElement->Get(errors, + "friction", this->dataPtr->friction).first; + this->dataPtr->springReference = dynElement->Get(errors, + "spring_reference", this->dataPtr->springReference).first; + this->dataPtr->springStiffness = dynElement->Get(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("lower", + this->dataPtr->lower = limitElement->Get(errors, "lower", this->dataPtr->lower).first; - this->dataPtr->upper = limitElement->Get("upper", + this->dataPtr->upper = limitElement->Get(errors, "upper", this->dataPtr->upper).first; - this->dataPtr->effort = limitElement->Get("effort", + this->dataPtr->effort = limitElement->Get(errors, "effort", this->dataPtr->effort).first; - this->dataPtr->maxVelocity = limitElement->Get("velocity", + this->dataPtr->maxVelocity = limitElement->Get(errors, "velocity", this->dataPtr->maxVelocity).first; - this->dataPtr->stiffness = limitElement->Get("stiffness", + this->dataPtr->stiffness = limitElement->Get(errors, "stiffness", this->dataPtr->stiffness).first; - this->dataPtr->dissipation = limitElement->Get("dissipation", - this->dataPtr->dissipation).first; + this->dataPtr->dissipation = limitElement->Get(errors, + "dissipation", this->dataPtr->dissipation).first; } else { @@ -376,6 +377,16 @@ 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); @@ -383,28 +394,36 @@ sdf::ElementPtr JointAxis::ToElement(unsigned int _index) const 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(this->Xyz()); + sdf::ElementPtr axisElem = elem->GetElement(axisElemName, _errors); + sdf::ElementPtr xyzElem = axisElem->GetElement("xyz", _errors); + xyzElem->Set(_errors, this->Xyz()); if (!this->XyzExpressedIn().empty()) { xyzElem->GetAttribute("expressed_in")->Set( - this->XyzExpressedIn()); + this->XyzExpressedIn(), _errors); } - sdf::ElementPtr dynElem = axisElem->GetElement("dynamics"); - dynElem->GetElement("damping")->Set(this->Damping()); - dynElem->GetElement("friction")->Set(this->Friction()); - dynElem->GetElement("spring_reference")->Set( - this->SpringReference()); - dynElem->GetElement("spring_stiffness")->Set( - this->SpringStiffness()); - - sdf::ElementPtr limitElem = axisElem->GetElement("limit"); - limitElem->GetElement("lower")->Set(this->Lower()); - limitElem->GetElement("upper")->Set(this->Upper()); - limitElem->GetElement("effort")->Set(this->Effort()); - limitElem->GetElement("velocity")->Set(this->MaxVelocity()); - limitElem->GetElement("stiffness")->Set(this->Stiffness()); - limitElem->GetElement("dissipation")->Set(this->Dissipation()); + sdf::ElementPtr dynElem = axisElem->GetElement("dynamics", _errors); + dynElem->GetElement("damping", _errors)->Set( + _errors, this->Damping()); + dynElem->GetElement("friction", _errors)->Set( + _errors, this->Friction()); + dynElem->GetElement("spring_reference", _errors)->Set( + _errors, this->SpringReference()); + dynElem->GetElement("spring_stiffness", _errors)->Set( + _errors, this->SpringStiffness()); + + sdf::ElementPtr limitElem = axisElem->GetElement("limit", _errors); + limitElem->GetElement("lower", _errors)->Set( + _errors, this->Lower()); + limitElem->GetElement("upper", _errors)->Set( + _errors, this->Upper()); + limitElem->GetElement("effort", _errors)->Set( + _errors, this->Effort()); + limitElem->GetElement("velocity", _errors)->Set( + _errors, this->MaxVelocity()); + limitElem->GetElement("stiffness", _errors)->Set( + _errors, this->Stiffness()); + limitElem->GetElement("dissipation", _errors)->Set( + _errors, this->Dissipation()); return axisElem; } diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index 031c989a6..b367ac01c 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -18,6 +18,7 @@ #include #include #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( + errors, "xyz", xyz).first; + ASSERT_TRUE(errors.empty()); + EXPECT_EQ(gz::math::Vector3d::UnitY, axis.Xyz()); + std::string expressedIn = elem->GetElement("xyz", errors)->Get( + 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(errors, "damping", damping).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(0.2, damping); + + double friction; + friction = dynElem->Get(errors, "friction", friction).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(1.3, friction); + + double springReference; + springReference = dynElem->Get( + errors, "spring_reference", springReference).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(2.4, springReference); + + double springStiffness; + springStiffness = dynElem->Get( + 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(errors, "lower", lower).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(-10.8, lower); + + double upper; + upper = limitElem->Get(errors, "upper", upper).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(123.4, upper); + + double effort; + effort = limitElem->Get(errors, "effort", effort).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(3.2, effort); + + double maxVel; + maxVel = limitElem->Get(errors, "velocity", maxVel).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(54.2, maxVel); + + double stiffness; + stiffness = limitElem->Get(errors, "stiffness", stiffness).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(1e2, stiffness); + + double dissipation; + dissipation = limitElem->Get( + 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(); +} From cde899a9346e2cc3be9131f9d799f7723532db04 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Tue, 28 Mar 2023 17:33:28 +0800 Subject: [PATCH 2/6] Imu: update calls to use sdf::Errors output (#1152) Signed-off-by: Marco A. Gutierrez --- include/sdf/Imu.hh | 8 +++ src/Imu.cc | 167 +++++++++++++++++++++++++++------------------ src/Imu_TEST.cc | 67 ++++++++++++++++++ 3 files changed, 175 insertions(+), 67 deletions(-) diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index c6afdd769..e3ced1181 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -260,6 +260,14 @@ namespace sdf /// \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/Imu.cc b/src/Imu.cc index 4fb31515a..65ea2e6e6 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -18,6 +18,7 @@ #include #include "sdf/Imu.hh" #include "sdf/parser.hh" +#include "Utils.hh" using namespace sdf; @@ -96,31 +97,34 @@ Errors Imu::Load(ElementPtr _sdf) // Load the linear acceleration noise values. if (_sdf->HasElement("linear_acceleration")) { - sdf::ElementPtr elem = _sdf->GetElement("linear_acceleration"); + sdf::ElementPtr elem = _sdf->GetElement("linear_acceleration", errors); if (elem->HasElement("x")) { - if (elem->GetElement("x")->HasElement("noise")) + if (elem->GetElement("x", errors)->HasElement("noise")) { - this->dataPtr->linearAccelXNoise.Load( - elem->GetElement("x")->GetElement("noise")); + sdf::Errors loadErrors = this->dataPtr->linearAccelXNoise.Load( + elem->GetElement("x", errors)->GetElement("noise", errors)); + errors.insert(errors.end(), loadErrors.begin(), loadErrors.end()); } } if (elem->HasElement("y")) { - if (elem->GetElement("y")->HasElement("noise")) + if (elem->GetElement("y", errors)->HasElement("noise")) { - this->dataPtr->linearAccelYNoise.Load( - elem->GetElement("y")->GetElement("noise")); + sdf::Errors loadErrors = this->dataPtr->linearAccelYNoise.Load( + elem->GetElement("y", errors)->GetElement("noise", errors)); + errors.insert(errors.end(), loadErrors.begin(), loadErrors.end()); } } if (elem->HasElement("z")) { - if (elem->GetElement("z")->HasElement("noise")) + if (elem->GetElement("z", errors)->HasElement("noise")) { - this->dataPtr->linearAccelZNoise.Load( - elem->GetElement("z")->GetElement("noise")); + sdf::Errors loadErrors = this->dataPtr->linearAccelZNoise.Load( + elem->GetElement("z", errors)->GetElement("noise", errors)); + errors.insert(errors.end(), loadErrors.begin(), loadErrors.end()); } } } @@ -128,64 +132,69 @@ Errors Imu::Load(ElementPtr _sdf) // Load the angular velocity noise values. if (_sdf->HasElement("angular_velocity")) { - sdf::ElementPtr elem = _sdf->GetElement("angular_velocity"); + sdf::ElementPtr elem = _sdf->GetElement("angular_velocity", errors); if (elem->HasElement("x")) { - if (elem->GetElement("x")->HasElement("noise")) + if (elem->GetElement("x", errors)->HasElement("noise")) { - this->dataPtr->angularVelXNoise.Load( - elem->GetElement("x")->GetElement("noise")); + sdf::Errors loadErrors = this->dataPtr->angularVelXNoise.Load( + elem->GetElement("x", errors)->GetElement("noise", errors)); + errors.insert(errors.end(), loadErrors.begin(), loadErrors.end()); } } if (elem->HasElement("y")) { - if (elem->GetElement("y")->HasElement("noise")) + if (elem->GetElement("y", errors)->HasElement("noise")) { - this->dataPtr->angularVelYNoise.Load( - elem->GetElement("y")->GetElement("noise")); + sdf::Errors loadErrors = this->dataPtr->angularVelYNoise.Load( + elem->GetElement("y", errors)->GetElement("noise", errors)); + errors.insert(errors.end(), loadErrors.begin(), loadErrors.end()); } } if (elem->HasElement("z")) { - if (elem->GetElement("z")->HasElement("noise")) + if (elem->GetElement("z", errors)->HasElement("noise")) { - this->dataPtr->angularVelZNoise.Load( - elem->GetElement("z")->GetElement("noise")); + sdf::Errors loadErrors = this->dataPtr->angularVelZNoise.Load( + elem->GetElement("z", errors)->GetElement("noise", errors)); + errors.insert(errors.end(), loadErrors.begin(), loadErrors.end()); } } } if (_sdf->HasElement("orientation_reference_frame")) { - sdf::ElementPtr elem = _sdf->GetElement("orientation_reference_frame"); - this->dataPtr->localization = elem->Get("localization", - this->dataPtr->localization).first; + sdf::ElementPtr elem = _sdf->GetElement( + "orientation_reference_frame", errors); + this->dataPtr->localization = elem->Get( + errors, "localization", this->dataPtr->localization).first; if (elem->HasElement("grav_dir_x")) { this->dataPtr->gravityDirX = elem->Get( - "grav_dir_x", this->dataPtr->gravityDirX).first; + errors, "grav_dir_x", this->dataPtr->gravityDirX).first; this->dataPtr->gravityDirXParentFrame = - elem->GetElement("grav_dir_x")->Get("parent_frame", + elem->GetElement("grav_dir_x", errors)->Get( + errors, "parent_frame", this->dataPtr->gravityDirXParentFrame).first; } if (elem->HasElement("custom_rpy")) { this->dataPtr->customRpy = elem->Get( - "custom_rpy", this->dataPtr->customRpy).first; + errors, "custom_rpy", this->dataPtr->customRpy).first; this->dataPtr->customRpyParentFrame = - elem->GetElement("custom_rpy")->Get("parent_frame", - this->dataPtr->customRpyParentFrame).first; + elem->GetElement("custom_rpy", errors)->Get( + errors, "parent_frame", this->dataPtr->customRpyParentFrame).first; } } if (_sdf->HasElement("enable_orientation")) { this->dataPtr->orientationEnabled = _sdf->Get( - "enable_orientation", this->dataPtr->orientationEnabled).first; + errors, "enable_orientation", this->dataPtr->orientationEnabled).first; } return errors; @@ -369,55 +378,79 @@ bool Imu::OrientationEnabled() const ///////////////////////////////////////////////// sdf::ElementPtr Imu::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Imu::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("imu.sdf", elem); sdf::ElementPtr orientationRefFrameElem = - elem->GetElement("orientation_reference_frame"); - orientationRefFrameElem->GetElement("localization")->Set( - this->Localization()); + elem->GetElement("orientation_reference_frame", _errors); + orientationRefFrameElem->GetElement( + "localization", _errors)->Set( + _errors, this->Localization()); sdf::ElementPtr customRPY = - orientationRefFrameElem->GetElement("custom_rpy"); - customRPY->Set(this->CustomRpy()); + orientationRefFrameElem->GetElement("custom_rpy", _errors); + customRPY->Set(_errors, this->CustomRpy()); customRPY->GetAttribute("parent_frame")->Set( - this->CustomRpyParentFrame()); + this->CustomRpyParentFrame(), _errors); sdf::ElementPtr gravDirX = - orientationRefFrameElem->GetElement("grav_dir_x"); - gravDirX->Set(this->GravityDirX()); + orientationRefFrameElem->GetElement("grav_dir_x", _errors); + gravDirX->Set(_errors, this->GravityDirX()); gravDirX->GetAttribute("parent_frame")->Set( - this->GravityDirXParentFrame()); - - sdf::ElementPtr angularVelElem = elem->GetElement("angular_velocity"); - sdf::ElementPtr angularVelXElem = angularVelElem->GetElement("x"); - sdf::ElementPtr angularVelXNoiseElem = angularVelXElem->GetElement("noise"); - angularVelXNoiseElem->Copy(this->dataPtr->angularVelXNoise.ToElement()); - - sdf::ElementPtr angularVelYElem = angularVelElem->GetElement("y"); - sdf::ElementPtr angularVelYNoiseElem = angularVelYElem->GetElement("noise"); - angularVelYNoiseElem->Copy(this->dataPtr->angularVelYNoise.ToElement()); - - sdf::ElementPtr angularVelZElem = angularVelElem->GetElement("z"); - sdf::ElementPtr angularVelZNoiseElem = angularVelZElem->GetElement("noise"); - angularVelZNoiseElem->Copy(this->dataPtr->angularVelZNoise.ToElement()); - - sdf::ElementPtr linearAccElem = elem->GetElement("linear_acceleration"); - sdf::ElementPtr linearAccXElem = linearAccElem->GetElement("x"); - sdf::ElementPtr linearAccXNoiseElem = linearAccXElem->GetElement("noise"); - linearAccXNoiseElem->Copy(this->dataPtr->linearAccelXNoise.ToElement()); - - sdf::ElementPtr linearAccYElem = linearAccElem->GetElement("y"); - sdf::ElementPtr linearAccYNoiseElem = linearAccYElem->GetElement("noise"); - linearAccYNoiseElem->Copy(this->dataPtr->linearAccelYNoise.ToElement()); - - sdf::ElementPtr linearAccZElem = linearAccElem->GetElement("z"); - sdf::ElementPtr linearAccZNoiseElem = linearAccZElem->GetElement("noise"); - linearAccZNoiseElem->Copy(this->dataPtr->linearAccelZNoise.ToElement()); - - elem->GetElement("enable_orientation")->Set( - this->OrientationEnabled()); + this->GravityDirXParentFrame(), _errors); + + sdf::ElementPtr angularVelElem = elem->GetElement( + "angular_velocity", _errors); + sdf::ElementPtr angularVelXElem = angularVelElem->GetElement("x", _errors); + sdf::ElementPtr angularVelXNoiseElem = angularVelXElem->GetElement( + "noise", _errors); + angularVelXNoiseElem->Copy( + this->dataPtr->angularVelXNoise.ToElement(_errors), _errors); + + sdf::ElementPtr angularVelYElem = angularVelElem->GetElement("y", _errors); + sdf::ElementPtr angularVelYNoiseElem = angularVelYElem->GetElement( + "noise", _errors); + angularVelYNoiseElem->Copy( + this->dataPtr->angularVelYNoise.ToElement(_errors), _errors); + + sdf::ElementPtr angularVelZElem = angularVelElem->GetElement("z", _errors); + sdf::ElementPtr angularVelZNoiseElem = angularVelZElem->GetElement( + "noise", _errors); + angularVelZNoiseElem->Copy(this->dataPtr->angularVelZNoise.ToElement( + _errors), _errors); + + sdf::ElementPtr linearAccElem = elem->GetElement( + "linear_acceleration", _errors); + sdf::ElementPtr linearAccXElem = linearAccElem->GetElement("x", _errors); + sdf::ElementPtr linearAccXNoiseElem = linearAccXElem->GetElement( + "noise", _errors); + linearAccXNoiseElem->Copy(this->dataPtr->linearAccelXNoise.ToElement( + _errors), _errors); + + sdf::ElementPtr linearAccYElem = linearAccElem->GetElement("y", _errors); + sdf::ElementPtr linearAccYNoiseElem = linearAccYElem->GetElement( + "noise", _errors); + linearAccYNoiseElem->Copy(this->dataPtr->linearAccelYNoise.ToElement( + _errors), _errors); + + sdf::ElementPtr linearAccZElem = linearAccElem->GetElement("z", _errors); + sdf::ElementPtr linearAccZNoiseElem = linearAccZElem->GetElement( + "noise", _errors); + linearAccZNoiseElem->Copy(this->dataPtr->linearAccelZNoise.ToElement( + _errors), _errors); + + elem->GetElement("enable_orientation", _errors)->Set( + _errors, this->OrientationEnabled()); return elem; } diff --git a/src/Imu_TEST.cc b/src/Imu_TEST.cc index 0398a2155..a532e2f29 100644 --- a/src/Imu_TEST.cc +++ b/src/Imu_TEST.cc @@ -17,6 +17,7 @@ #include #include "sdf/Imu.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(DOMImu, Construction) @@ -181,3 +182,69 @@ TEST(DOMImu, ToElement) imu3.Load(imu2Elem); EXPECT_EQ(gz::math::Vector3d(1, 2, 3), imu3.GravityDirX()); } + +///////////////////////////////////////////////// +TEST(DOMImu, ToElementErrorOutput) +{ + 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 + + // test calling ToElement on a DOM object constructed without calling Load + sdf::Imu imu; + sdf::Noise noise; + sdf::Errors errors; + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + imu.SetLinearAccelerationXNoise(noise); + imu.SetLinearAccelerationYNoise(noise); + imu.SetLinearAccelerationZNoise(noise); + imu.SetAngularVelocityXNoise(noise); + imu.SetAngularVelocityYNoise(noise); + imu.SetAngularVelocityZNoise(noise); + imu.SetGravityDirX(gz::math::Vector3d::Zero); + imu.SetGravityDirXParentFrame("my_frame"); + imu.SetCustomRpy(gz::math::Vector3d::UnitZ); + imu.SetCustomRpyParentFrame("other_frame"); + imu.SetLocalization("NED"); + imu.SetOrientationEnabled(false); + + sdf::ElementPtr imuElem = imu.ToElement(errors); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, imuElem); + EXPECT_EQ(nullptr, imu.Element()); + + // verify values after loading the element back + sdf::Imu imu2; + errors = imu2.Load(imuElem); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(noise, imu2.LinearAccelerationXNoise()); + EXPECT_EQ(noise, imu2.LinearAccelerationYNoise()); + EXPECT_EQ(noise, imu2.LinearAccelerationZNoise()); + EXPECT_EQ(noise, imu2.AngularVelocityXNoise()); + EXPECT_EQ(noise, imu2.AngularVelocityYNoise()); + EXPECT_EQ(noise, imu2.AngularVelocityZNoise()); + EXPECT_EQ(gz::math::Vector3d::Zero, imu2.GravityDirX()); + EXPECT_EQ("my_frame", imu2.GravityDirXParentFrame()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, imu2.CustomRpy()); + EXPECT_EQ("other_frame", imu2.CustomRpyParentFrame()); + EXPECT_EQ("NED", imu2.Localization()); + EXPECT_FALSE(imu2.OrientationEnabled()); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} From 16457d54af31c5d83c74cd665fffbd42b7930569 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Tue, 28 Mar 2023 18:38:10 +0800 Subject: [PATCH 3/6] Light: update calls to use sdf::Errors parameters (#1154) * updating Element calls to use Errors Signed-off-by: Marco A. Gutierrez * Light: adding error parameter to function calls Signed-off-by: Marco A. Gutierrez * Adding ToElement errors test Signed-off-by: Marco A. Gutierrez --------- Signed-off-by: Marco A. Gutierrez --- include/sdf/Light.hh | 8 ++++ src/Light.cc | 109 ++++++++++++++++++++++++------------------- src/Light_TEST.cc | 69 +++++++++++++++++++++++++++ 3 files changed, 139 insertions(+), 47 deletions(-) diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index 69a3d788f..fc19e02f1 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -291,6 +291,14 @@ namespace sdf /// \return SDF element pointer with updated light values. public: sdf::ElementPtr ToElement() const; + /// \brief Create and return an SDF element filled with data from this + /// light object. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[out] _errors Vector of errors. + /// \return SDF element pointer with updated light values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const; + /// \brief Allow Link::SetPoseRelativeToGraph or World::Load to call /// SetXmlParentName and SetPoseRelativeToGraph, /// but Link::SetPoseRelativeToGraph is a private function, so we need diff --git a/src/Light.cc b/src/Light.cc index 0bbc48e8b..02fb48001 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -115,7 +115,7 @@ Errors Light::Load(ElementPtr _sdf) return errors; } - std::string typeString = _sdf->Get("type", + std::string typeString = _sdf->Get(errors, "type", std::string("point")).first; if (typeString == "point") this->dataPtr->type = LightType::POINT; @@ -148,29 +148,29 @@ Errors Light::Load(ElementPtr _sdf) // Load the pose. Ignore the return value since the light pose is optional. loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); - this->dataPtr->isLightOn = _sdf->Get("light_on", + this->dataPtr->isLightOn = _sdf->Get(errors, "light_on", this->dataPtr->isLightOn).first; - this->dataPtr->visualize = _sdf->Get("visualize", + this->dataPtr->visualize = _sdf->Get(errors, "visualize", this->dataPtr->visualize).first; - this->dataPtr->castShadows = _sdf->Get("cast_shadows", + this->dataPtr->castShadows = _sdf->Get(errors, "cast_shadows", this->dataPtr->castShadows).first; - this->dataPtr->intensity = _sdf->Get("intensity", + this->dataPtr->intensity = _sdf->Get(errors, "intensity", this->dataPtr->intensity).first; - this->dataPtr->diffuse = _sdf->Get("diffuse", + this->dataPtr->diffuse = _sdf->Get(errors, "diffuse", this->dataPtr->diffuse).first; - this->dataPtr->specular = _sdf->Get("specular", + this->dataPtr->specular = _sdf->Get(errors, "specular", this->dataPtr->specular).first; - sdf::ElementPtr attenuationElem = _sdf->GetElement("attenuation"); + sdf::ElementPtr attenuationElem = _sdf->GetElement("attenuation", errors); if (attenuationElem) { std::pair doubleValue = attenuationElem->Get( - "range", this->dataPtr->attenuationRange); + errors, "range", this->dataPtr->attenuationRange); if (!doubleValue.second) { errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -178,14 +178,14 @@ Errors Light::Load(ElementPtr _sdf) } this->SetAttenuationRange(doubleValue.first); - this->SetLinearAttenuationFactor(attenuationElem->Get("linear", - this->dataPtr->linearAttenuation).first); + this->SetLinearAttenuationFactor(attenuationElem->Get( + errors, "linear", this->dataPtr->linearAttenuation).first); - this->SetConstantAttenuationFactor(attenuationElem->Get("constant", - this->dataPtr->constantAttenuation).first); + this->SetConstantAttenuationFactor(attenuationElem->Get( + errors, "constant", this->dataPtr->constantAttenuation).first); this->SetQuadraticAttenuationFactor(attenuationElem->Get( - "quadratic", this->dataPtr->quadraticAttenuation).first); + errors, "quadratic", this->dataPtr->quadraticAttenuation).first); } // Read the direction @@ -193,7 +193,7 @@ Errors Light::Load(ElementPtr _sdf) this->dataPtr->type == LightType::DIRECTIONAL) { std::pair dirPair = - _sdf->Get<>("direction", this->dataPtr->direction); + _sdf->Get<>(errors, "direction", this->dataPtr->direction); if (!dirPair.second) { @@ -204,12 +204,12 @@ Errors Light::Load(ElementPtr _sdf) this->dataPtr->direction = dirPair.first; } - sdf::ElementPtr spotElem = _sdf->GetElement("spot"); + sdf::ElementPtr spotElem = _sdf->GetElement("spot", errors); if (this->dataPtr->type == LightType::SPOT && spotElem) { // Check for and set inner_angle std::pair doubleValue = spotElem->Get( - "inner_angle", this->dataPtr->spotInnerAngle.Radian()); + errors, "inner_angle", this->dataPtr->spotInnerAngle.Radian()); if (!doubleValue.second) { errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -219,7 +219,7 @@ Errors Light::Load(ElementPtr _sdf) // Check for and set outer_angle doubleValue = spotElem->Get( - "outer_angle", this->dataPtr->spotOuterAngle.Radian()); + errors, "outer_angle", this->dataPtr->spotOuterAngle.Radian()); if (!doubleValue.second) { errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -228,7 +228,8 @@ Errors Light::Load(ElementPtr _sdf) this->SetSpotOuterAngle(doubleValue.first); // Check for and set falloff - doubleValue = spotElem->Get("falloff", this->dataPtr->spotFalloff); + doubleValue = spotElem->Get( + errors, "falloff", this->dataPtr->spotFalloff); if (!doubleValue.second) { errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -488,6 +489,15 @@ void Light::SetType(const LightType _type) ///////////////////////////////////////////////// sdf::ElementPtr Light::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Light::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("light.sdf", elem); @@ -507,37 +517,42 @@ sdf::ElementPtr Light::ToElement() const default: break; } - elem->GetAttribute("type")->Set(lightTypeStr); - elem->GetAttribute("name")->Set(this->Name()); - sdf::ElementPtr poseElem = elem->GetElement("pose"); + elem->GetAttribute("type")->Set(lightTypeStr, _errors); + elem->GetAttribute("name")->Set(this->Name(), _errors); + sdf::ElementPtr poseElem = elem->GetElement("pose", _errors); if (!this->dataPtr->poseRelativeTo.empty()) { poseElem->GetAttribute("relative_to")->Set( - this->dataPtr->poseRelativeTo); + this->dataPtr->poseRelativeTo, _errors); } - poseElem->Set(this->RawPose()); - - elem->GetElement("cast_shadows")->Set(this->CastShadows()); - elem->GetElement("intensity")->Set(this->Intensity()); - elem->GetElement("direction")->Set( - this->Direction()); - elem->GetElement("diffuse")->Set(this->Diffuse()); - elem->GetElement("specular")->Set(this->Specular()); - sdf::ElementPtr attenuationElem = elem->GetElement("attenuation"); - attenuationElem->GetElement("linear")->Set( - this->LinearAttenuationFactor()); - attenuationElem->GetElement("constant")->Set( - this->ConstantAttenuationFactor()); - attenuationElem->GetElement("quadratic")->Set( - this->QuadraticAttenuationFactor()); - attenuationElem->GetElement("range")->Set( - this->AttenuationRange()); - - sdf::ElementPtr spotElem = elem->GetElement("spot"); - spotElem->GetElement("inner_angle")->Set( - this->SpotInnerAngle().Radian()); - spotElem->GetElement("outer_angle")->Set( - this->SpotOuterAngle().Radian()); - spotElem->GetElement("falloff")->Set(this->SpotFalloff()); + poseElem->Set(_errors, this->RawPose()); + + elem->GetElement("cast_shadows", _errors)->Set( + _errors, this->CastShadows()); + elem->GetElement("intensity", _errors)->Set( + _errors, this->Intensity()); + elem->GetElement("direction", _errors)->Set( + _errors, this->Direction()); + elem->GetElement("diffuse", _errors)->Set( + _errors, this->Diffuse()); + elem->GetElement("specular", _errors)->Set( + _errors, this->Specular()); + sdf::ElementPtr attenuationElem = elem->GetElement("attenuation", _errors); + attenuationElem->GetElement("linear", _errors)->Set( + _errors, this->LinearAttenuationFactor()); + attenuationElem->GetElement("constant", _errors)->Set( + _errors, this->ConstantAttenuationFactor()); + attenuationElem->GetElement("quadratic", _errors)->Set( + _errors, this->QuadraticAttenuationFactor()); + attenuationElem->GetElement("range", _errors)->Set( + _errors, this->AttenuationRange()); + + sdf::ElementPtr spotElem = elem->GetElement("spot", _errors); + spotElem->GetElement("inner_angle", _errors)->Set( + _errors, this->SpotInnerAngle().Radian()); + spotElem->GetElement("outer_angle", _errors)->Set( + _errors, this->SpotOuterAngle().Radian()); + spotElem->GetElement("falloff", _errors)->Set( + _errors, this->SpotFalloff()); return elem; } diff --git a/src/Light_TEST.cc b/src/Light_TEST.cc index e17309ebc..70c705596 100644 --- a/src/Light_TEST.cc +++ b/src/Light_TEST.cc @@ -18,6 +18,7 @@ #include #include #include "sdf/Light.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(DOMLight, DefaultConstruction) @@ -400,3 +401,71 @@ TEST(DOMLight, ToElement) light3.Load(light2Elem); EXPECT_FALSE(light3.CastShadows()); } + +///////////////////////////////////////////////// +TEST(DOMLight, ToElementErrorOutput) +{ + 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 + + // test calling ToElement on a DOM object constructed without calling Load + sdf::Light light; + light.SetName("test_light_assignment"); + light.SetType(sdf::LightType::SPOT); + light.SetRawPose({3, 2, 1, 0, GZ_PI, 0}); + light.SetPoseRelativeTo("ground_plane"); + light.SetCastShadows(true); + light.SetDiffuse(gz::math::Color(0.4f, 0.5f, 0.6f, 1.0)); + light.SetSpecular(gz::math::Color(0.8f, 0.9f, 0.1f, 1.0)); + light.SetAttenuationRange(3.2); + light.SetLinearAttenuationFactor(0.1); + light.SetConstantAttenuationFactor(0.5); + light.SetQuadraticAttenuationFactor(0.01); + light.SetDirection({0.1, 0.2, 1}); + light.SetSpotInnerAngle(1.9); + light.SetSpotOuterAngle(3.3); + light.SetSpotFalloff(0.9); + light.SetIntensity(1.7); + + sdf::Errors errors; + sdf::ElementPtr lightElem = light.ToElement(errors); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, lightElem); + EXPECT_EQ(nullptr, light.Element()); + + // verify values after loading the element back + sdf::Light light2; + errors = light2.Load(lightElem); + EXPECT_TRUE(errors.empty()); + + EXPECT_NE(nullptr, light2.Element()); + EXPECT_EQ("test_light_assignment", light2.Name()); + EXPECT_EQ(sdf::LightType::SPOT, light2.Type()); + EXPECT_EQ(gz::math::Pose3d(3, 2, 1, 0, GZ_PI, 0), light2.RawPose()); + EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); + EXPECT_TRUE(light2.CastShadows()); + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); + EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); + EXPECT_DOUBLE_EQ(0.1, light2.LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.5, light2.ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light2.QuadraticAttenuationFactor()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 1), light2.Direction()); + EXPECT_EQ(gz::math::Angle(1.9), light2.SpotInnerAngle()); + EXPECT_EQ(gz::math::Angle(3.3), light2.SpotOuterAngle()); + EXPECT_DOUBLE_EQ(0.9, light2.SpotFalloff()); + EXPECT_DOUBLE_EQ(1.7, light2.Intensity()); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} From 319ec31a5acefcd51f12d7c935794eb7f9df6ad3 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Tue, 28 Mar 2023 19:58:47 +0800 Subject: [PATCH 4/6] Material: update calls to use sdf::Errors output (#1155) Signed-off-by: Marco A. Gutierrez --- include/sdf/Material.hh | 8 ++ src/Material.cc | 167 ++++++++++++++++++++++++---------------- src/Material_TEST.cc | 138 +++++++++++++++++++++++++++++++++ 3 files changed, 245 insertions(+), 68 deletions(-) diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index ae3e6339e..6891a8899 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -218,6 +218,14 @@ namespace sdf /// \return SDF element pointer with updated material values. public: sdf::ElementPtr ToElement() const; + /// \brief Create and return an SDF element filled with data from this + /// material. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[out] _errors Vector of errors. + /// \return SDF element pointer with updated material values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const; + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/Material.cc b/src/Material.cc index c5c41c394..0a0b77c42 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -109,8 +109,9 @@ Errors Material::Load(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config) // Load the script information if (_sdf->HasElement("script")) { - sdf::ElementPtr elem = _sdf->GetElement("script"); - std::pair uriPair = elem->Get("uri", ""); + sdf::ElementPtr elem = _sdf->GetElement("script", errors); + std::pair uriPair = + elem->Get(errors, "uri", ""); if (uriPair.first == "__default__") uriPair.first = ""; @@ -123,7 +124,8 @@ Errors Material::Load(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config) this->dataPtr->scriptUri = resolveURI(uriPair.first, _config, errors); - std::pair namePair = elem->Get("name", ""); + std::pair namePair = + elem->Get(errors, "name", ""); if (namePair.first == "__default__") namePair.first = ""; @@ -139,10 +141,10 @@ Errors Material::Load(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config) // Load the shader information if (_sdf->HasElement("shader")) { - sdf::ElementPtr elem = _sdf->GetElement("shader"); + sdf::ElementPtr elem = _sdf->GetElement("shader", errors); std::pair typePair = - elem->Get("type", "pixel"); + elem->Get(errors, "type", "pixel"); if (typePair.first == "pixel") this->dataPtr->shader = ShaderType::PIXEL; else if (typePair.first == "vertex") @@ -162,7 +164,8 @@ Errors Material::Load(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config) "not supported"}); } - this->dataPtr->normalMap = elem->Get("normal_map", "").first; + this->dataPtr->normalMap = + elem->Get(errors, "normal_map", "").first; if (this->dataPtr->normalMap == "__default__") this->dataPtr->normalMap = ""; @@ -176,35 +179,36 @@ Errors Material::Load(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config) } } - this->dataPtr->renderOrder = _sdf->Get("render_order", + this->dataPtr->renderOrder = _sdf->Get(errors, "render_order", this->dataPtr->renderOrder).first; - this->dataPtr->ambient = _sdf->Get("ambient", + this->dataPtr->ambient = _sdf->Get(errors, "ambient", this->dataPtr->ambient).first; - this->dataPtr->diffuse = _sdf->Get("diffuse", + this->dataPtr->diffuse = _sdf->Get(errors, "diffuse", this->dataPtr->diffuse).first; - this->dataPtr->specular = _sdf->Get("specular", + this->dataPtr->specular = _sdf->Get(errors, "specular", this->dataPtr->specular).first; - this->dataPtr->shininess = _sdf->Get("shininess", + this->dataPtr->shininess = _sdf->Get(errors, "shininess", this->dataPtr->shininess).first; - this->dataPtr->emissive = _sdf->Get("emissive", + this->dataPtr->emissive = _sdf->Get(errors, "emissive", this->dataPtr->emissive).first; - this->dataPtr->lighting = _sdf->Get("lighting", + this->dataPtr->lighting = _sdf->Get(errors, "lighting", this->dataPtr->lighting).first; - this->dataPtr->doubleSided = _sdf->Get("double_sided", + this->dataPtr->doubleSided = _sdf->Get(errors, "double_sided", this->dataPtr->doubleSided).first; // load pbr param if (_sdf->HasElement("pbr")) { this->dataPtr->pbr.emplace(); - Errors pbrErrors = this->dataPtr->pbr->Load(_sdf->GetElement("pbr")); + Errors pbrErrors = this->dataPtr->pbr->Load( + _sdf->GetElement("pbr", errors)); errors.insert(errors.end(), pbrErrors.begin(), pbrErrors.end()); } @@ -387,102 +391,129 @@ void Material::SetFilePath(const std::string &_filePath) ///////////////////////////////////////////////// sdf::ElementPtr Material::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Material::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("material.sdf", elem); - elem->GetElement("ambient")->Set(this->Ambient()); - elem->GetElement("diffuse")->Set(this->Diffuse()); - elem->GetElement("specular")->Set(this->Specular()); - elem->GetElement("emissive")->Set(this->Emissive()); - elem->GetElement("render_order")->Set(this->RenderOrder()); - elem->GetElement("lighting")->Set(this->Lighting()); - elem->GetElement("double_sided")->Set(this->DoubleSided()); + elem->GetElement("ambient", _errors)->Set(_errors, this->Ambient()); + elem->GetElement("diffuse", _errors)->Set(_errors, this->Diffuse()); + elem->GetElement("specular", _errors)->Set(_errors, this->Specular()); + elem->GetElement("emissive", _errors)->Set(_errors, this->Emissive()); + elem->GetElement("render_order", _errors)->Set(_errors, this->RenderOrder()); + elem->GetElement("lighting", _errors)->Set(_errors, this->Lighting()); + elem->GetElement("double_sided", _errors)->Set(_errors, this->DoubleSided()); // Script, if set if (!this->ScriptName().empty() && !this->ScriptUri().empty()) { - sdf::ElementPtr scriptElem = elem->GetElement("script"); - scriptElem->GetElement("uri")->Set(this->ScriptUri()); - scriptElem->GetElement("name")->Set(this->ScriptName()); + sdf::ElementPtr scriptElem = elem->GetElement("script", _errors); + scriptElem->GetElement("uri", _errors)->Set(_errors, this->ScriptUri()); + scriptElem->GetElement("name", _errors)->Set(_errors, this->ScriptName()); } // Shader properties - sdf::ElementPtr shaderElem = elem->GetElement("shader"); + sdf::ElementPtr shaderElem = elem->GetElement("shader", _errors); switch (this->dataPtr->shader) { default: case ShaderType::PIXEL: - shaderElem->GetAttribute("type")->Set("pixel"); + shaderElem->GetAttribute("type")->Set("pixel", _errors); break; case ShaderType::VERTEX: - shaderElem->GetAttribute("type")->Set("vertex"); + shaderElem->GetAttribute("type")->Set("vertex", _errors); break; case ShaderType::NORMAL_MAP_OBJECTSPACE: - shaderElem->GetAttribute("type")->Set("normal_map_object_space"); + shaderElem->GetAttribute("type")->Set( + "normal_map_object_space", _errors); break; case ShaderType::NORMAL_MAP_TANGENTSPACE: - shaderElem->GetAttribute("type")->Set("normal_map_tangent_space"); + shaderElem->GetAttribute("type")->Set( + "normal_map_tangent_space", _errors); break; } if (!this->NormalMap().empty()) - shaderElem->GetElement("normal_map")->Set(this->NormalMap()); + shaderElem->GetElement("normal_map", _errors)->Set( + _errors, this->NormalMap()); // PBR material if (this->dataPtr->pbr) { const PbrWorkflow *workflow = this->dataPtr->pbr->Workflow( PbrWorkflowType::METAL); - sdf::ElementPtr pbrElem = elem->GetElement("pbr"); + sdf::ElementPtr pbrElem = elem->GetElement("pbr", _errors); if (workflow && workflow->Type() == PbrWorkflowType::METAL) { - sdf::ElementPtr metalElem = pbrElem->GetElement("metal"); - metalElem->GetElement("albedo_map")->Set(workflow->AlbedoMap()); - metalElem->GetElement("roughness_map")->Set(workflow->RoughnessMap()); - metalElem->GetElement("roughness")->Set(workflow->Roughness()); - metalElem->GetElement("metalness_map")->Set(workflow->MetalnessMap()); - metalElem->GetElement("metalness")->Set(workflow->Metalness()); - metalElem->GetElement("ambient_occlusion_map")->Set( - workflow->AmbientOcclusionMap()); - sdf::ElementPtr normalElem = metalElem->GetElement("normal_map"); + sdf::ElementPtr metalElem = pbrElem->GetElement("metal", _errors); + metalElem->GetElement("albedo_map", _errors)->Set( + _errors, workflow->AlbedoMap()); + metalElem->GetElement("roughness_map", _errors)->Set( + _errors, workflow->RoughnessMap()); + metalElem->GetElement("roughness", _errors)->Set( + _errors, workflow->Roughness()); + metalElem->GetElement("metalness_map", _errors)->Set( + _errors, workflow->MetalnessMap()); + metalElem->GetElement("metalness", _errors)->Set( + _errors, workflow->Metalness()); + metalElem->GetElement("ambient_occlusion_map", _errors)->Set( + _errors, workflow->AmbientOcclusionMap()); + sdf::ElementPtr normalElem = metalElem->GetElement( + "normal_map", _errors); if (workflow->NormalMapType() == NormalMapSpace::TANGENT) - normalElem->GetAttribute("type")->Set("tangent"); + normalElem->GetAttribute("type")->Set("tangent", _errors); else - normalElem->GetAttribute("type")->Set("object"); - normalElem->Set(workflow->NormalMap()); + normalElem->GetAttribute("type")->Set("object", _errors); + normalElem->Set(_errors, workflow->NormalMap()); - metalElem->GetElement("emissive_map")->Set(workflow->EmissiveMap()); + metalElem->GetElement("emissive_map", _errors)->Set( + _errors, workflow->EmissiveMap()); - sdf::ElementPtr lightElem = metalElem->GetElement("light_map"); - lightElem->GetAttribute("uv_set")->Set(workflow->LightMapTexCoordSet()); - lightElem->Set(workflow->LightMap()); + sdf::ElementPtr lightElem = metalElem->GetElement("light_map", _errors); + lightElem->GetAttribute("uv_set")->Set( + workflow->LightMapTexCoordSet(), _errors); + lightElem->Set(_errors, workflow->LightMap()); } workflow = this->dataPtr->pbr->Workflow(PbrWorkflowType::SPECULAR); if (workflow && workflow->Type() == PbrWorkflowType::SPECULAR) { - sdf::ElementPtr specularElem = pbrElem->GetElement("specular"); - specularElem->GetElement("albedo_map")->Set(workflow->AlbedoMap()); - specularElem->GetElement("specular_map")->Set(workflow->SpecularMap()); - specularElem->GetElement("environment_map")->Set( - workflow->EnvironmentMap()); - specularElem->GetElement("ambient_occlusion_map")->Set( - workflow->AmbientOcclusionMap()); - specularElem->GetElement("emissive_map")->Set(workflow->EmissiveMap()); - specularElem->GetElement("glossiness_map")->Set( - workflow->GlossinessMap()); - specularElem->GetElement("glossiness")->Set(workflow->Glossiness()); - - sdf::ElementPtr normalElem = specularElem->GetElement("normal_map"); + sdf::ElementPtr specularElem = pbrElem->GetElement("specular", _errors); + specularElem->GetElement("albedo_map", _errors)->Set( + _errors, workflow->AlbedoMap()); + specularElem->GetElement("specular_map", _errors)->Set( + _errors, workflow->SpecularMap()); + specularElem->GetElement("environment_map", _errors)->Set( + _errors, workflow->EnvironmentMap()); + specularElem->GetElement("ambient_occlusion_map", _errors)->Set( + _errors, workflow->AmbientOcclusionMap()); + specularElem->GetElement("emissive_map", _errors)->Set( + _errors, workflow->EmissiveMap()); + specularElem->GetElement("glossiness_map", _errors)->Set( + _errors, workflow->GlossinessMap()); + specularElem->GetElement("glossiness", _errors)->Set( + _errors, workflow->Glossiness()); + + sdf::ElementPtr normalElem = specularElem->GetElement( + "normal_map", _errors); if (workflow->NormalMapType() == NormalMapSpace::TANGENT) - normalElem->GetAttribute("type")->Set("tangent"); + normalElem->GetAttribute("type")->Set("tangent", _errors); else - normalElem->GetAttribute("type")->Set("object"); - normalElem->Set(workflow->NormalMap()); - - sdf::ElementPtr lightElem = specularElem->GetElement("light_map"); - lightElem->GetAttribute("uv_set")->Set(workflow->LightMapTexCoordSet()); - lightElem->Set(workflow->LightMap()); + normalElem->GetAttribute("type")->Set("object", _errors); + normalElem->Set(_errors, workflow->NormalMap()); + + sdf::ElementPtr lightElem = specularElem->GetElement( + "light_map", _errors); + lightElem->GetAttribute("uv_set")->Set( + workflow->LightMapTexCoordSet(), _errors); + lightElem->Set(_errors, workflow->LightMap()); } } diff --git a/src/Material_TEST.cc b/src/Material_TEST.cc index 4dfc2c8be..6a4e174c9 100644 --- a/src/Material_TEST.cc +++ b/src/Material_TEST.cc @@ -20,6 +20,7 @@ #include #include "sdf/Material.hh" #include "sdf/Pbr.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(DOMMaterial, Construction) @@ -418,3 +419,140 @@ TEST(DOMMaterial, ToElement) EXPECT_EQ(flow1->Type(), flow2->Type()); } } + +///////////////////////////////////////////////// +TEST(DOMMaterial, ToElementErrorOutput) +{ + 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::Material material; + sdf::Errors errors; + + material.SetAmbient(gz::math::Color(0.1f, 0.2f, 0.3f, 1.0f)); + material.SetDiffuse(gz::math::Color(0.4f, 0.5f, 0.6f, 0.5f)); + material.SetSpecular(gz::math::Color(0.6f, 0.4f, 0.8f, 0.8f)); + material.SetEmissive(gz::math::Color(0.2f, 0.7f, 0.9f, 0.1f)); + + material.SetRenderOrder(0.5); + material.SetLighting(false); + material.SetDoubleSided(true); + material.SetScriptUri("my-uri"); + material.SetScriptName("my-script-name"); + material.SetShader(sdf::ShaderType::VERTEX); + material.SetNormalMap("my-normal-map"); + + sdf::PbrWorkflow workflow; + workflow.SetAlbedoMap("albedo"); + workflow.SetNormalMap("normal"); + workflow.SetEnvironmentMap("env"); + workflow.SetAmbientOcclusionMap("ambient"); + workflow.SetRoughnessMap("rough"); + workflow.SetMetalnessMap("metalness"); + workflow.SetEmissiveMap("emissive"); + workflow.SetGlossinessMap("gloss"); + workflow.SetSpecularMap("gloss"); + workflow.SetLightMap("light", 1u); + workflow.SetMetalness(1.0); + workflow.SetRoughness(0.1); + + // Testing using METAL workflow + { + sdf::Pbr pbr; + workflow.SetType(sdf::PbrWorkflowType::METAL); + pbr.SetWorkflow(sdf::PbrWorkflowType::METAL, workflow); + material.SetPbrMaterial(pbr); + + sdf::ElementPtr elem = material.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Material material2; + errors = material2.Load(elem); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(material.Ambient(), material2.Ambient()); + EXPECT_EQ(material.Diffuse(), material2.Diffuse()); + EXPECT_EQ(material.Specular(), material2.Specular()); + EXPECT_EQ(material.Emissive(), material2.Emissive()); + EXPECT_DOUBLE_EQ(material.RenderOrder(), material2.RenderOrder()); + EXPECT_EQ(material.Lighting(), material2.Lighting()); + EXPECT_EQ(material.DoubleSided(), material2.DoubleSided()); + EXPECT_EQ(material.ScriptUri(), material2.ScriptUri()); + EXPECT_EQ(material.ScriptName(), material2.ScriptName()); + EXPECT_EQ(material.Shader(), material2.Shader()); + EXPECT_EQ(material.NormalMap(), material2.NormalMap()); + + const sdf::Pbr *pbr2 = material2.PbrMaterial(); + const sdf::PbrWorkflow *flow1 = pbr.Workflow(sdf::PbrWorkflowType::METAL); + const sdf::PbrWorkflow *flow2 = pbr2->Workflow(sdf::PbrWorkflowType::METAL); + EXPECT_EQ(flow1->AlbedoMap(), flow2->AlbedoMap()); + EXPECT_EQ(flow1->NormalMap(), flow2->NormalMap()); + EXPECT_EQ(flow1->AmbientOcclusionMap(), + flow2->AmbientOcclusionMap()); + EXPECT_EQ(flow1->RoughnessMap(), flow2->RoughnessMap()); + EXPECT_EQ(flow1->MetalnessMap(), flow2->MetalnessMap()); + EXPECT_EQ(flow1->EmissiveMap(), flow2->EmissiveMap()); + EXPECT_EQ(flow1->LightMap(), flow2->LightMap()); + EXPECT_DOUBLE_EQ(flow1->Metalness(), flow2->Metalness()); + EXPECT_DOUBLE_EQ(flow1->Roughness(), flow2->Roughness()); + EXPECT_EQ(flow1->Type(), flow2->Type()); + } + + // Testing using SPECULAR workflow + { + sdf::Pbr pbr; + workflow.SetType(sdf::PbrWorkflowType::SPECULAR); + pbr.SetWorkflow(sdf::PbrWorkflowType::SPECULAR, workflow); + material.SetPbrMaterial(pbr); + + sdf::ElementPtr elem = material.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Material material2; + errors = material2.Load(elem); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(material.Ambient(), material2.Ambient()); + EXPECT_EQ(material.Diffuse(), material2.Diffuse()); + EXPECT_EQ(material.Specular(), material2.Specular()); + EXPECT_EQ(material.Emissive(), material2.Emissive()); + EXPECT_DOUBLE_EQ(material.RenderOrder(), material2.RenderOrder()); + EXPECT_EQ(material.Lighting(), material2.Lighting()); + EXPECT_EQ(material.DoubleSided(), material2.DoubleSided()); + EXPECT_EQ(material.ScriptUri(), material2.ScriptUri()); + EXPECT_EQ(material.ScriptName(), material2.ScriptName()); + EXPECT_EQ(material.Shader(), material2.Shader()); + EXPECT_EQ(material.NormalMap(), material2.NormalMap()); + + const sdf::Pbr *pbr2 = material2.PbrMaterial(); + const sdf::PbrWorkflow *flow1 = + pbr.Workflow(sdf::PbrWorkflowType::SPECULAR); + const sdf::PbrWorkflow *flow2 = + pbr2->Workflow(sdf::PbrWorkflowType::SPECULAR); + EXPECT_EQ(flow1->AlbedoMap(), flow2->AlbedoMap()); + EXPECT_EQ(flow1->NormalMap(), flow2->NormalMap()); + EXPECT_EQ(flow1->EnvironmentMap(), flow2->EnvironmentMap()); + EXPECT_EQ(flow1->AmbientOcclusionMap(), + flow2->AmbientOcclusionMap()); + EXPECT_EQ(flow1->EmissiveMap(), flow2->EmissiveMap()); + EXPECT_EQ(flow1->GlossinessMap(), flow2->GlossinessMap()); + EXPECT_EQ(flow1->SpecularMap(), flow2->SpecularMap()); + EXPECT_EQ(flow1->LightMap(), flow2->LightMap()); + EXPECT_EQ(flow1->Type(), flow2->Type()); + } + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} From 9ab7799db1ce264c1026aa76296b8502ff9917ee Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Tue, 28 Mar 2023 21:35:13 +0800 Subject: [PATCH 5/6] Frame: update calls to use sdf::Errors output (#1156) Signed-off-by: Marco A. Gutierrez --- include/sdf/Frame.hh | 8 ++++++ src/Frame.cc | 21 ++++++++++---- src/Frame_TEST.cc | 68 ++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 91 insertions(+), 6 deletions(-) diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index e2aa56bc8..cecb63ea9 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -131,6 +131,14 @@ namespace sdf /// \return SDF element pointer with updated frame values. public: sdf::ElementPtr ToElement() const; + /// \brief Create and return an SDF element filled with data from this + /// frame. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[out] _errors Vector of errors. + /// \return SDF element pointer with updated frame values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const; + /// \brief Give a scoped FrameAttachedToGraph to be used for resolving /// attached bodies. This is private and is intended to be called by /// Model::Load or World::Load. diff --git a/src/Frame.cc b/src/Frame.cc index dc4dd1705..de9870915 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -95,7 +95,7 @@ Errors Frame::Load(ElementPtr _sdf) // Read the frame's attached_to attribute if (_sdf->HasAttribute("attached_to")) { - auto pair = _sdf->Get("attached_to", ""); + auto pair = _sdf->Get(errors, "attached_to", ""); if (pair.second) { this->dataPtr->attachedTo = pair.first; @@ -221,23 +221,32 @@ sdf::ElementPtr Frame::Element() const ///////////////////////////////////////////////// sdf::ElementPtr Frame::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Frame::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("frame.sdf", elem); - elem->GetAttribute("name")->Set(this->dataPtr->name); + elem->GetAttribute("name")->Set(this->dataPtr->name, _errors); if (!this->dataPtr->attachedTo.empty()) - elem->GetAttribute("attached_to")->Set(this->dataPtr->attachedTo); + elem->GetAttribute("attached_to")->Set(this->dataPtr->attachedTo, _errors); // Set pose - sdf::ElementPtr poseElem = elem->GetElement("pose"); + sdf::ElementPtr poseElem = elem->GetElement("pose", _errors); if (!this->dataPtr->poseRelativeTo.empty()) { poseElem->GetAttribute("relative_to")->Set( - this->dataPtr->poseRelativeTo); + this->dataPtr->poseRelativeTo, _errors); } - poseElem->Set(this->RawPose()); + poseElem->Set(_errors, this->RawPose()); return elem; } diff --git a/src/Frame_TEST.cc b/src/Frame_TEST.cc index 7bcc6a313..f2afdfb47 100644 --- a/src/Frame_TEST.cc +++ b/src/Frame_TEST.cc @@ -18,6 +18,7 @@ #include #include "sdf/Frame.hh" #include "sdf/Geometry.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(DOMframe, Construction) @@ -116,3 +117,70 @@ TEST(DOMFrame, ToElement) EXPECT_TRUE(frame2.AttachedTo().empty()); } } + +///////////////////////////////////////////////// +TEST(DOMFrame, ToElementErrorOutput) +{ + 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::Errors errors; + + // With 'attached-to' + { + sdf::Frame frame; + + frame.SetName("my-frame"); + frame.SetAttachedTo("attached-to-frame"); + frame.SetPoseRelativeTo("relative-to-frame"); + frame.SetRawPose(gz::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + + sdf::ElementPtr elem = frame.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Frame frame2; + errors = frame2.Load(elem); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(frame.Name(), frame.Name()); + EXPECT_EQ(frame.AttachedTo(), frame2.AttachedTo()); + EXPECT_EQ(frame.PoseRelativeTo(), frame2.PoseRelativeTo()); + EXPECT_EQ(frame.RawPose(), frame2.RawPose()); + } + + // Without 'attached-to' + { + sdf::Frame frame; + + frame.SetName("my-frame"); + frame.SetRawPose(gz::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + EXPECT_TRUE(frame.AttachedTo().empty()); + + sdf::ElementPtr elem = frame.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Frame frame2; + errors = frame2.Load(elem); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(frame.Name(), frame.Name()); + EXPECT_EQ(frame.RawPose(), frame2.RawPose()); + EXPECT_TRUE(frame.AttachedTo().empty()); + EXPECT_TRUE(frame2.AttachedTo().empty()); + } + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} From 0f6ca9a02d0ff2538c458da0f4e54587534fef6a Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 29 Mar 2023 10:33:16 +0800 Subject: [PATCH 6/6] Physics: update calls to use sdf::Errors output (#1157) Signed-off-by: Marco A. Gutierrez --- include/sdf/Physics.hh | 8 ++++++++ src/Physics.cc | 35 ++++++++++++++++++++++---------- src/Physics_TEST.cc | 46 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 78 insertions(+), 11 deletions(-) diff --git a/include/sdf/Physics.hh b/include/sdf/Physics.hh index e20eebcef..0fa5ea5ef 100644 --- a/include/sdf/Physics.hh +++ b/include/sdf/Physics.hh @@ -121,6 +121,14 @@ namespace sdf /// \return SDF element pointer with updated physics values. public: sdf::ElementPtr ToElement() const; + /// \brief Create and return an SDF element filled with data from this + /// physics. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[out] _errors Vector of errors. + /// \return SDF element pointer with updated physics values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const; + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/Physics.cc b/src/Physics.cc index 86f8b2e7d..b9dd69317 100644 --- a/src/Physics.cc +++ b/src/Physics.cc @@ -86,10 +86,10 @@ Errors Physics::Load(sdf::ElementPtr _sdf) } this->dataPtr->isDefault = - _sdf->Get("default", this->dataPtr->isDefault).first; + _sdf->Get(errors, "default", this->dataPtr->isDefault).first; std::pair stringPair = - _sdf->Get("type", this->dataPtr->type); + _sdf->Get(errors, "type", this->dataPtr->type); if (!stringPair.second) { errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -99,7 +99,7 @@ Errors Physics::Load(sdf::ElementPtr _sdf) this->dataPtr->type = stringPair.first; std::pair doublePair = - _sdf->Get("max_step_size", this->dataPtr->stepSize); + _sdf->Get(errors, "max_step_size", this->dataPtr->stepSize); if (!doublePair.second) { errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -108,7 +108,8 @@ Errors Physics::Load(sdf::ElementPtr _sdf) } this->dataPtr->stepSize = doublePair.first; - doublePair = _sdf->Get("real_time_factor", this->dataPtr->rtf); + doublePair = _sdf->Get( + errors, "real_time_factor", this->dataPtr->rtf); if (!doublePair.second) { errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -118,7 +119,7 @@ Errors Physics::Load(sdf::ElementPtr _sdf) this->dataPtr->rtf = doublePair.first; this->dataPtr->maxContacts = - _sdf->Get("max_contacts", this->dataPtr->maxContacts).first; + _sdf->Get(errors, "max_contacts", this->dataPtr->maxContacts).first; return errors; } @@ -203,17 +204,29 @@ void Physics::SetMaxContacts(int _maxContacts) ///////////////////////////////////////////////// sdf::ElementPtr Physics::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Physics::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("physics.sdf", elem); - elem->GetAttribute("name")->Set(this->Name()); - elem->GetAttribute("default")->Set(this->IsDefault()); - elem->GetAttribute("type")->Set(this->EngineType()); + elem->GetAttribute("name")->Set(this->Name(), _errors); + elem->GetAttribute("default")->Set(this->IsDefault(), _errors); + elem->GetAttribute("type")->Set(this->EngineType(), _errors); - elem->GetElement("max_step_size")->Set(this->MaxStepSize()); - elem->GetElement("real_time_factor")->Set(this->RealTimeFactor()); - elem->GetElement("max_contacts")->Set(this->MaxContacts()); + elem->GetElement("max_step_size", _errors)->Set( + _errors, this->MaxStepSize()); + elem->GetElement("real_time_factor", _errors)->Set( + _errors, this->RealTimeFactor()); + elem->GetElement("max_contacts", _errors)->Set( + _errors, this->MaxContacts()); /// \todo(nkoenig) Support engine specific parameters. diff --git a/src/Physics_TEST.cc b/src/Physics_TEST.cc index 43bf95fa0..25ad32b02 100644 --- a/src/Physics_TEST.cc +++ b/src/Physics_TEST.cc @@ -17,6 +17,7 @@ #include #include "sdf/Physics.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(DOMPhysics, DefaultConstruction) @@ -131,3 +132,48 @@ TEST(DOMPhysics, ToElement) EXPECT_DOUBLE_EQ(physics.RealTimeFactor(), physics2.RealTimeFactor()); EXPECT_EQ(physics.MaxContacts(), physics2.MaxContacts()); } + +///////////////////////////////////////////////// +TEST(DOMPhysics, ToElementErrorOutput) +{ + 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::Physics physics; + sdf::Errors errors; + physics.SetName("my-bullet-engine"); + physics.SetDefault(true); + physics.SetEngineType("bullet"); + physics.SetMaxStepSize(0.1); + physics.SetRealTimeFactor(20.4); + physics.SetMaxContacts(42); + + sdf::ElementPtr elem = physics.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Physics physics2; + errors = physics2.Load(elem); + EXPECT_TRUE(errors.empty()); + + // verify values after loading the element back + EXPECT_EQ(physics.Name(), physics2.Name()); + EXPECT_EQ(physics.IsDefault(), physics2.IsDefault()); + EXPECT_EQ(physics.EngineType(), physics2.EngineType()); + EXPECT_DOUBLE_EQ(physics.MaxStepSize(), physics2.MaxStepSize()); + EXPECT_DOUBLE_EQ(physics.RealTimeFactor(), physics2.RealTimeFactor()); + EXPECT_EQ(physics.MaxContacts(), physics2.MaxContacts()); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +}