From add21ad920014d0aa627792141c3641629d4f3b9 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) # Conflicts: # src/Sensor.cc # src/Surface.cc --- 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 | 116 +++++++++++++++++++++++++++++++----------- src/Surface.cc | 9 ++++ 8 files changed, 206 insertions(+), 35 deletions(-) diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index 104f88051..6f55b6948 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -45,7 +45,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 74bbd415b..034c52377 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. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/src/Geometry.cc b/src/Geometry.cc index ceae9cd72..e0a8fced3 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -36,7 +36,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 8ab7d7e64..117b1f85f 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 3f4c2ab3c..e859d1b62 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 fc146beff..10455f7fc 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -71,7 +71,7 @@ const std::vector sensorTypeStrs = class sdf::Sensor::Implementation { - // \brief The sensor type. + /// \brief The sensor type. public: SensorType type = SensorType::NONE; /// \brief Name of the sensor. @@ -701,72 +701,128 @@ sdf::ElementPtr Sensor::ToElement() 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()); +<<<<<<< HEAD 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()); +>>>>>>> 8ccc60cd (NavSat::ToElement() method (#1585)) // 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); + } + } +<<<<<<< HEAD +======= + // air speed + else if (this->Type() == sdf::SensorType::AIR_SPEED) + { + 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); + } } +>>>>>>> 8ccc60cd (NavSat::ToElement() method (#1585)) // 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) + { + 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) { - sdf::ElementPtr magnetometerElem = elem->GetElement("magnetometer"); - magnetometerElem->Copy(this->dataPtr->magnetometer->ToElement()); + if (this->dataPtr->navSat) + { + sdf::ElementPtr navSatElem = elem->GetElement("navsat"); + navSatElem->Copy(this->dataPtr->navSat->ToElement(_errors), _errors); + } } else { +<<<<<<< HEAD std::cout << "Conversion of sensor type: [" << this->TypeStr() << "] from " << "SDF DOM to Element is not supported yet." << std::endl; +======= + std::stringstream ss; + ss << "Conversion of sensor type: [" << this->TypeStr() << "] from SDF " + "DOM to Element is not supported yet." << this->Name() << '\n'; + _errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); +>>>>>>> 8ccc60cd (NavSat::ToElement() method (#1585)) } // 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 57f05f95b..ce8a43508 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -30,9 +30,18 @@ using namespace sdf; class sdf::Contact::Implementation { +<<<<<<< HEAD // \brief The bitmask used to filter collisions. public: uint16_t collideBitmask = 0xff; +======= + /// \brief The collide bitmask used to filter collisions. + public: uint16_t collideBitmask = 0xff; + + /// \brief The category bitmask used to filter collisions. + public: std::optional categoryBitmask; + +>>>>>>> 8ccc60cd (NavSat::ToElement() method (#1585)) /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf{nullptr}; };