diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index 499664a046..f10d16f0ba 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -134,6 +134,10 @@ class gz::sim::systems::MagnetometerPrivate /// True if the rendering component is initialized public: bool initialized = false; + /// True if the magnetic field should be reported in teslas + /// See: https://github.com/gazebosim/gz-sim/issues/2312 + public: bool useTeslaForMagneticField = false; + /// \brief Create sensor /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU @@ -231,6 +235,12 @@ class gz::sim::systems::MagnetometerPrivate { return get_table_data(lat, lon, strength_table); } + + // convert magnetic field strength from gauss to tesla + float gauss_to_tesla(const float gauss) + { + return gauss / 10'000; + } }; ////////////////////////////////////////////////// @@ -242,6 +252,20 @@ Magnetometer::Magnetometer() : System(), dataPtr( ////////////////////////////////////////////////// Magnetometer::~Magnetometer() = default; +////////////////////////////////////////////////// +void Magnetometer::Configure(const Entity &/*_entity*/, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + // If the SDF specifies it, convert the gauss measurements to tesla + if (_sdf->HasElement("use_tesla_for_magnetic_field")) + { + this->dataPtr->useTeslaForMagneticField = + _sdf->Get("use_tesla_for_magnetic_field"); + } +} + ////////////////////////////////////////////////// void Magnetometer::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) @@ -430,7 +454,7 @@ void MagnetometerPrivate::Update( auto latLonEle = sphericalCoordinates(_entity, _ecm); if (!latLonEle) { - gzwarn << "Failed to update NavSat sensor enity [" << _entity + gzwarn << "Failed to update NavSat sensor entity [" << _entity << "]. Spherical coordinates not set." << std::endl; return true; } @@ -451,6 +475,11 @@ void MagnetometerPrivate::Update( 0.01f * get_mag_strength(lat_rad * 180 / GZ_PI, lon_rad * 180 / GZ_PI); + if (useTeslaForMagneticField) + { + strength_ga = gauss_to_tesla(strength_ga); + } + // Magnetic filed components are calculated by http://geomag.nrcan.gc.ca/mag_fld/comp-en.php float H = strength_ga * cosf(inclination_rad); float Z = tanf(inclination_rad) * H; @@ -494,6 +523,7 @@ void MagnetometerPrivate::RemoveMagnetometerEntities( } GZ_ADD_PLUGIN(Magnetometer, System, + Magnetometer::ISystemConfigure, Magnetometer::ISystemPreUpdate, Magnetometer::ISystemPostUpdate ) diff --git a/src/systems/magnetometer/Magnetometer.hh b/src/systems/magnetometer/Magnetometer.hh index 854eb44a3b..e2e1c5d166 100644 --- a/src/systems/magnetometer/Magnetometer.hh +++ b/src/systems/magnetometer/Magnetometer.hh @@ -37,6 +37,7 @@ namespace systems /// current location. class Magnetometer: public System, + public ISystemConfigure, public ISystemPreUpdate, public ISystemPostUpdate { @@ -46,6 +47,12 @@ namespace systems /// \brief Destructor public: ~Magnetometer() override; + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/test/integration/magnetometer_system.cc b/test/integration/magnetometer_system.cc index 38daa6f972..8c3ecd5b40 100644 --- a/test/integration/magnetometer_system.cc +++ b/test/integration/magnetometer_system.cc @@ -14,6 +14,10 @@ * limitations under the License. * */ +#include +#include +#include +#include #include @@ -66,8 +70,8 @@ TEST_F(MagnetometerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RotatedMagnetometer)) { // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/magnetometer.sdf"; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "/test/worlds/magnetometer.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); @@ -141,3 +145,148 @@ TEST_F(MagnetometerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RotatedMagnetometer)) field.Z(), TOL); mutex.unlock(); } + + +///////////////////////////////////////////////// +// Test to check to ensure that the fix for using tesla units works correctly +// Note(gilbert): It seems the above test (RotatedMagnetometer) +// does not actually run because the spherical coordinates are not set. +// In order to not break the above tests, we create a new world and run +// two simulations, one with gauss and the other with tesla units. +// See https://github.com/gazebosim/gz-sim/issues/2312 + +class MagnetometerMessageBuffer { +public: + void add(const msgs::Magnetometer &_msg) + { + std::lock_guard lock(mutex); + magnetometerMsgs.push_back(_msg); + } + + msgs::Magnetometer at(const size_t index) const + { + std::lock_guard lock(mutex); + return magnetometerMsgs.at(index); + } + + size_t size() const + { + std::lock_guard lock(mutex); + return magnetometerMsgs.size(); + } + +private: + mutable std::mutex mutex; + std::vector magnetometerMsgs; +}; + +MagnetometerMessageBuffer gaussMessages; +MagnetometerMessageBuffer teslaMessages; + +///////////////////////////////////////////////// +void gaussMagnetometerCb(const msgs::Magnetometer &_msg) +{ + gaussMessages.add(_msg); +} + +///////////////////////////////////////////////// +void teslaMagnetometerCb(const msgs::Magnetometer &_msg) +{ + teslaMessages.add(_msg); +} + +TEST_F(MagnetometerTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(GaussToTeslaCorrection)) +{ + // First, run the world without the fix enabled and record the magnetic field. + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "/test/worlds/magnetometer_with_tesla.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server gaussServer(serverConfig); + EXPECT_FALSE(gaussServer.Running()); + EXPECT_FALSE(*gaussServer.Running(0)); + + const auto topic = "world/magnetometer_sensor/model/magnetometer_model/" + "link/link/sensor/magnetometer_sensor/magnetometer"; + + // subscribe to magnetometer topic + std::unique_ptr gaussNode = + std::make_unique(); + gaussNode->Subscribe(topic, &gaussMagnetometerCb); + + // step world and verify magnetometer's detected field + // Run the gauss server + const size_t iters = 100u; + gaussServer.Run(true, iters, false); + + // Now, we need to create a new server where the magnetometer + // is correctly publishing using tesla. + // Flip the use_tesla_for_magnetic_field field to true + sdf::SDFPtr teslaWorldSdf(new sdf::SDF()); + sdf::init(teslaWorldSdf); + ASSERT_TRUE(sdf::readFile(sdfFile, teslaWorldSdf)); + sdf::ElementPtr root = teslaWorldSdf->Root(); + EXPECT_TRUE(root->HasElement("world")); + + sdf::ElementPtr world = root->GetElement("world"); + const std::string pluginName = "gz::sim::systems::Magnetometer"; + const std::string useTeslaElementName = "use_tesla_for_magnetic_field"; + bool elementSet = false; + for (sdf::ElementPtr plugin = world->GetElement("plugin"); + plugin; + plugin = plugin->GetNextElement("plugin")) + { + if (plugin->Get("name") == pluginName) + { + // Found the magnetometer plugin, now force the field to true + if (plugin->HasElement(useTeslaElementName)) + { + sdf::ElementPtr teslaElement = plugin->GetElement(useTeslaElementName); + teslaElement->Set(true); + elementSet = true; + break; + } + } + } + EXPECT_TRUE(elementSet); + + // Now, re-run the world but with the tesla units being published + EXPECT_TRUE(serverConfig.SetSdfString(teslaWorldSdf->ToString())); + + Server teslaServer(serverConfig); + EXPECT_FALSE(teslaServer.Running()); + EXPECT_FALSE(*teslaServer.Running(0)); + + // subscribe to tesla magnetometer topic and unsubscribe from the gauss topic + gaussNode = nullptr; + std::unique_ptr teslaNode = + std::make_unique(); + teslaNode->Subscribe(topic, &teslaMagnetometerCb); + + // step world and verify magnetometer's detected field + // Run the tesla server + teslaServer.Run(true, iters, false); + + // Now compare the two sets of magnetic data + ASSERT_EQ(gaussMessages.size(), teslaMessages.size()); + + for (size_t index = 0; index < teslaMessages.size(); ++index) + { + const auto& gaussMessage = gaussMessages.at(index); + const auto& teslaMessage = teslaMessages.at(index); + + // 1 gauss is 10^-4 teslas + auto gauss_to_tesla = [&](const float gauss) -> float { + return gauss / 10'000; + }; + + EXPECT_FLOAT_EQ(gauss_to_tesla(gaussMessage.field_tesla().x()), + teslaMessage.field_tesla().x()); + EXPECT_FLOAT_EQ(gauss_to_tesla(gaussMessage.field_tesla().y()), + teslaMessage.field_tesla().y()); + EXPECT_FLOAT_EQ(gauss_to_tesla(gaussMessage.field_tesla().z()), + teslaMessage.field_tesla().z()); + } +} diff --git a/test/worlds/magnetometer_with_tesla.sdf b/test/worlds/magnetometer_with_tesla.sdf new file mode 100644 index 0000000000..0fc462a68c --- /dev/null +++ b/test/worlds/magnetometer_with_tesla.sdf @@ -0,0 +1,94 @@ + + + + 0.94 0.76 -0.12 + + 0.001 + 0 + + + + + false + + + + + + + EARTH_WGS84 + ENU + 37.3861 + -122.0839 + 0 + 0 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 1.570796327 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 1 + + + + + +