From 978a9a56df3f7edfce6d3f5f77b7f0e53bf29039 Mon Sep 17 00:00:00 2001 From: Maksim Derbasov Date: Wed, 4 Mar 2026 04:22:31 +0900 Subject: [PATCH] NavSat::ToElement() method (#1585) `sdf::NavSat` didn't have a `ToElement()` method which prevent to convert sensor-specific information back to sdf format. This PR include such changes: * Introduce `sdf::NavSat::ToElement()` method * Added this method call to dispatcher in `Sensor::ToElement()` * Also same dispatcher had misleading error msg: if user tried to convert sensor w/o sensor-specific information it will get error message about lack of this feature, but actually it's not correct. Dispatcher logic changed to avoid such misleading (split apart type check and pointer check) * And message was w/o trailing new_line symbol which makes console output harder to read * Added test for new ToElement() method; valgrind is happy * Couple of typos fixed --------- Signed-off-by: Maksim Derbasov (cherry picked from commit 8ccc60cd6e211c36828d282a86d18ea972e19c7e) --- include/sdf/Error.hh | 2 +- include/sdf/NavSat.hh | 15 ++++++ src/Geometry.cc | 2 +- src/NavSat.cc | 51 ++++++++++++++++++++ src/NavSat_TEST.cc | 40 +++++++++++++++ src/ScopedGraph.hh | 6 +-- src/Sensor.cc | 110 +++++++++++++++++++++++++++--------------- src/Surface.cc | 4 +- 8 files changed, 184 insertions(+), 46 deletions(-) diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index 0020808aa..a3bbc286e 100644 --- a/include/sdf/Error.hh +++ b/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. diff --git a/include/sdf/NavSat.hh b/include/sdf/NavSat.hh index 13341132e..3d1d83d69 100644 --- a/include/sdf/NavSat.hh +++ b/include/sdf/NavSat.hh @@ -133,6 +133,21 @@ namespace sdf /// \return True if 'this' != _navsat. public: bool operator!=(const NavSat &_navsat) const; + /// \brief Create and return an SDF element filled with data from this + /// 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 Create and return an SDF element filled with data from this + /// NavSat. + /// 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/Geometry.cc b/src/Geometry.cc index 8883f80ee..e430dd094 100644 --- a/src/Geometry.cc +++ b/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. diff --git a/src/NavSat.cc b/src/NavSat.cc index be9862738..2dc3278e5 100644 --- a/src/NavSat.cc +++ b/src/NavSat.cc @@ -15,6 +15,8 @@ * */ #include "sdf/NavSat.hh" +#include "sdf/parser.hh" +#include "Utils.hh" using namespace sdf; using namespace gz; @@ -191,3 +193,52 @@ bool NavSat::operator!=(const NavSat &_navsat) const { return !(*this == _navsat); } + +///////////////////////////////////////////////// +sdf::ElementPtr NavSat::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr NavSat::ToElement(sdf::Errors &_errors) const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("navsat.sdf", ParserConfig::GlobalConfig(), elem, _errors); + const auto defaultNoise = sdf::Noise(); + + if (this->dataPtr->horizontalPositionNoise != defaultNoise) + { + auto el = elem->GetElement("position_sensing", _errors)-> + GetElement("horizontal", _errors)->GetElement("noise", _errors); + el->Copy( + this->dataPtr->horizontalPositionNoise.ToElement(_errors), _errors); + } + + if (this->dataPtr->verticalPositionNoise != defaultNoise) + { + auto el = elem->GetElement("position_sensing", _errors)-> + GetElement("vertical", _errors)->GetElement("noise", _errors); + el->Copy(this->dataPtr->verticalPositionNoise.ToElement(_errors), _errors); + } + + if (this->dataPtr->horizontalVelocityNoise != defaultNoise) + { + auto el = elem->GetElement("velocity_sensing", _errors)-> + GetElement("horizontal", _errors)->GetElement("noise", _errors); + el->Copy( + this->dataPtr->horizontalVelocityNoise.ToElement(_errors), _errors); + } + + if (this->dataPtr->verticalVelocityNoise != defaultNoise) + { + auto el = elem->GetElement("velocity_sensing", _errors)-> + GetElement("vertical", _errors)->GetElement("noise", _errors); + el->Copy(this->dataPtr->verticalVelocityNoise.ToElement(_errors), _errors); + } + + return elem; +} diff --git a/src/NavSat_TEST.cc b/src/NavSat_TEST.cc index a4d10e925..bc79934e7 100644 --- a/src/NavSat_TEST.cc +++ b/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()); +} diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 41739f179..d75d3a5ab 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -139,9 +139,9 @@ class ScopedGraph /// \return A new child scope. public: ScopedGraph 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 RootScope() const; /// \brief Checks if the scope points to a valid graph. diff --git a/src/Sensor.cc b/src/Sensor.cc index 86d708e46..379047f0a 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -76,7 +76,7 @@ constexpr std::array kSensorTypeStrs = class sdf::Sensor::Implementation { - // \brief The sensor type. + /// \brief The sensor type. public: SensorType type = SensorType::NONE; /// \brief Name of the sensor. @@ -762,82 +762,114 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const 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()); - elem->GetElement("frame_id")->Set(this->FrameId()); - elem->GetElement("topic")->Set(this->Topic()); - elem->GetElement("update_rate")->Set(this->UpdateRate()); - elem->GetElement("enable_metrics")->Set(this->EnableMetrics()); + elem->GetElement("frame_id")->Set(_errors, this->FrameId()); + elem->GetElement("topic")->Set(_errors, this->Topic()); + elem->GetElement("update_rate")->Set(_errors, this->UpdateRate()); + elem->GetElement("enable_metrics")->Set(_errors, + 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(_errors), _errors); + } } // 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) + { + // TODO(anyone) use ToElement with sdf::Error when it available + sdf::ElementPtr airSpeedElem = elem->GetElement("air_speed"); + airSpeedElem->Copy(this->dataPtr->airSpeed->ToElement(), _errors); + } } // 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(_errors), _errors); + } } // camera, depth, thermal, segmentation else if (this->CameraSensor()) { + // TODO(anyone) use ToElement with sdf::Error when it available sdf::ElementPtr cameraElem = elem->GetElement("camera"); - cameraElem->Copy(this->dataPtr->camera->ToElement()); + cameraElem->Copy(this->dataPtr->camera->ToElement(), _errors); } // 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(_errors), _errors); + } } // 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(_errors), _errors); + } } // 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) + { + // TODO(anyone) use ToElement with sdf::Error when it available + sdf::ElementPtr rayElem = (elem->HasElement("ray")) ? + elem->GetElement("ray") : elem->GetElement("lidar"); + rayElem->Copy(this->dataPtr->lidar->ToElement(), _errors); + } } // 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) + { + // TODO(anyone) use ToElement with sdf::Error when it available + sdf::ElementPtr magnetometerElem = elem->GetElement("magnetometer"); + magnetometerElem->Copy( + this->dataPtr->magnetometer->ToElement(), _errors); + } + } + else if (this->Type() == sdf::SensorType::NAVSAT) + { + if (this->dataPtr->navSat) + { + sdf::ElementPtr navSatElem = elem->GetElement("navsat"); + navSatElem->Copy(this->dataPtr->navSat->ToElement(_errors), _errors); + } } 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()}); } // Add in the plugins for (const Plugin &plugin : this->dataPtr->plugins) - elem->InsertElement(plugin.ToElement(), true); + elem->InsertElement(plugin.ToElement(_errors), true); return elem; } diff --git a/src/Surface.cc b/src/Surface.cc index a13fa1823..889b1fbf0 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -30,10 +30,10 @@ using namespace sdf; class sdf::Contact::Implementation { - // \brief The collide bitmask used to filter collisions. + /// \brief The collide bitmask used to filter collisions. public: uint16_t collideBitmask = 0xff; - // \brief The category bitmask used to filter collisions. + /// \brief The category bitmask used to filter collisions. public: std::optional categoryBitmask; /// \brief The SDF element pointer used during load.