From 6d53c66436547a570e3479304737992b55fe96a6 Mon Sep 17 00:00:00 2001 From: Maksim Derbasov Date: Wed, 3 Sep 2025 02:27:51 +0900 Subject: [PATCH] NavSat ToElement() method Signed-off-by: Maksim Derbasov --- include/sdf/Error.hh | 2 +- include/sdf/NavSat.hh | 7 ++++ src/Geometry.cc | 2 +- src/NavSat.cc | 39 ++++++++++++++++++++ src/NavSat_TEST.cc | 40 ++++++++++++++++++++ src/ScopedGraph.hh | 6 +-- src/Sensor.cc | 85 +++++++++++++++++++++++++++---------------- src/Surface.cc | 2 +- 8 files changed, 146 insertions(+), 37 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..4d9b56bd1 100644 --- a/include/sdf/NavSat.hh +++ b/include/sdf/NavSat.hh @@ -133,6 +133,13 @@ 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 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..d5523e646 100644 --- a/src/NavSat.cc +++ b/src/NavSat.cc @@ -15,6 +15,7 @@ * */ #include "sdf/NavSat.hh" +#include "sdf/parser.hh" using namespace sdf; using namespace gz; @@ -191,3 +192,41 @@ bool NavSat::operator!=(const NavSat &_navsat) const { return !(*this == _navsat); } + +///////////////////////////////////////////////// +sdf::ElementPtr NavSat::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("navsat.sdf", elem); + const auto defaultNoise = sdf::Noise(); + + if (this->dataPtr->horizontalPositionNoise != defaultNoise) + { + auto el = elem->GetElement("position_sensing")-> + GetElement("horizontal")->GetElement("noise"); + el->Copy(this->dataPtr->horizontalPositionNoise.ToElement()); + } + + if (this->dataPtr->verticalPositionNoise != defaultNoise) + { + auto el = elem->GetElement("position_sensing")-> + GetElement("vertical")->GetElement("noise"); + el->Copy(this->dataPtr->verticalPositionNoise.ToElement()); + } + + if (this->dataPtr->horizontalVelocityNoise != defaultNoise) + { + auto el = elem->GetElement("velocity_sensing")-> + GetElement("horizontal")->GetElement("noise"); + el->Copy(this->dataPtr->horizontalVelocityNoise.ToElement()); + } + + if (this->dataPtr->verticalVelocityNoise != defaultNoise) + { + auto el = elem->GetElement("velocity_sensing")-> + GetElement("vertical")->GetElement("noise"); + el->Copy(this->dataPtr->verticalVelocityNoise.ToElement()); + } + + 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 8f3b12055..0695c8f1d 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. @@ -772,25 +772,31 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const elem->GetElement("enable_metrics")->Set(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()); + } } // 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) + { + sdf::ElementPtr airSpeedElem = elem->GetElement("air_speed"); + airSpeedElem->Copy(this->dataPtr->airSpeed->ToElement()); + } } // 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()); + } } // camera, depth, thermal, segmentation else if (this->CameraSensor()) @@ -799,39 +805,56 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const cameraElem->Copy(this->dataPtr->camera->ToElement()); } // 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()); + } } // 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()); + } } // 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) + { + sdf::ElementPtr rayElem = (elem->HasElement("ray")) ? + elem->GetElement("ray") : elem->GetElement("lidar"); + rayElem->Copy(this->dataPtr->lidar->ToElement()); + } } // 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) + { + sdf::ElementPtr magnetometerElem = elem->GetElement("magnetometer"); + magnetometerElem->Copy(this->dataPtr->magnetometer->ToElement()); + } + } + else if (this->Type() == sdf::SensorType::NAVSAT) + { + if (this->dataPtr->navSat) + { + sdf::ElementPtr navSatElem = elem->GetElement("navsat"); + navSatElem->Copy(this->dataPtr->navSat->ToElement()); + } } 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()}); } diff --git a/src/Surface.cc b/src/Surface.cc index a0343ecf5..a1ab90548 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -30,7 +30,7 @@ using namespace sdf; class sdf::Contact::Implementation { - // \brief The bitmask used to filter collisions. + /// \brief The bitmask used to filter collisions. public: uint16_t collideBitmask = 0xff; /// \brief The SDF element pointer used during load.