Skip to content

Commit

Permalink
Merge branch 'sdf13' into marcoag/sdf_error_light
Browse files Browse the repository at this point in the history
  • Loading branch information
Marco A. Gutierrez authored Mar 28, 2023
2 parents 5dea672 + cde899a commit ce487dd
Show file tree
Hide file tree
Showing 6 changed files with 354 additions and 106 deletions.
8 changes: 8 additions & 0 deletions include/sdf/Imu.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
};
Expand Down
10 changes: 10 additions & 0 deletions include/sdf/JointAxis.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
167 changes: 100 additions & 67 deletions src/Imu.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <string>
#include "sdf/Imu.hh"
#include "sdf/parser.hh"
#include "Utils.hh"

using namespace sdf;

Expand Down Expand Up @@ -96,96 +97,104 @@ 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());
}
}
}

// 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<std::string>("localization",
this->dataPtr->localization).first;
sdf::ElementPtr elem = _sdf->GetElement(
"orientation_reference_frame", errors);
this->dataPtr->localization = elem->Get<std::string>(
errors, "localization", this->dataPtr->localization).first;

if (elem->HasElement("grav_dir_x"))
{
this->dataPtr->gravityDirX = elem->Get<gz::math::Vector3d>(
"grav_dir_x", this->dataPtr->gravityDirX).first;
errors, "grav_dir_x", this->dataPtr->gravityDirX).first;
this->dataPtr->gravityDirXParentFrame =
elem->GetElement("grav_dir_x")->Get<std::string>("parent_frame",
elem->GetElement("grav_dir_x", errors)->Get<std::string>(
errors, "parent_frame",
this->dataPtr->gravityDirXParentFrame).first;
}

if (elem->HasElement("custom_rpy"))
{
this->dataPtr->customRpy = elem->Get<gz::math::Vector3d>(
"custom_rpy", this->dataPtr->customRpy).first;
errors, "custom_rpy", this->dataPtr->customRpy).first;
this->dataPtr->customRpyParentFrame =
elem->GetElement("custom_rpy")->Get<std::string>("parent_frame",
this->dataPtr->customRpyParentFrame).first;
elem->GetElement("custom_rpy", errors)->Get<std::string>(
errors, "parent_frame", this->dataPtr->customRpyParentFrame).first;
}
}

if (_sdf->HasElement("enable_orientation"))
{
this->dataPtr->orientationEnabled = _sdf->Get<bool>(
"enable_orientation", this->dataPtr->orientationEnabled).first;
errors, "enable_orientation", this->dataPtr->orientationEnabled).first;
}

return errors;
Expand Down Expand Up @@ -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<std::string>(
this->Localization());
elem->GetElement("orientation_reference_frame", _errors);
orientationRefFrameElem->GetElement(
"localization", _errors)->Set<std::string>(
_errors, this->Localization());

sdf::ElementPtr customRPY =
orientationRefFrameElem->GetElement("custom_rpy");
customRPY->Set<gz::math::Vector3d>(this->CustomRpy());
orientationRefFrameElem->GetElement("custom_rpy", _errors);
customRPY->Set<gz::math::Vector3d>(_errors, this->CustomRpy());
customRPY->GetAttribute("parent_frame")->Set<std::string>(
this->CustomRpyParentFrame());
this->CustomRpyParentFrame(), _errors);

sdf::ElementPtr gravDirX =
orientationRefFrameElem->GetElement("grav_dir_x");
gravDirX->Set<gz::math::Vector3d>(this->GravityDirX());
orientationRefFrameElem->GetElement("grav_dir_x", _errors);
gravDirX->Set<gz::math::Vector3d>(_errors, this->GravityDirX());
gravDirX->GetAttribute("parent_frame")->Set<std::string>(
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<bool>(
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<bool>(
_errors, this->OrientationEnabled());

return elem;
}
67 changes: 67 additions & 0 deletions src/Imu_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <gtest/gtest.h>
#include "sdf/Imu.hh"
#include "test_utils.hh"

/////////////////////////////////////////////////
TEST(DOMImu, Construction)
Expand Down Expand Up @@ -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();
}
Loading

0 comments on commit ce487dd

Please sign in to comment.