Add magnetometer plugin parameter to publish units as tesla

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<const sdf::Element> &_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<bool>("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
 )

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<const sdf::Element> &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &_eventMgr) override;
+
  /// Documentation inherited
  public: void PreUpdate(const UpdateInfo &_info,
  EntityComponentManager &_ecm) final;

test/integration/magnetometer_system.cc

@@ -14,6 +14,10 @@
  * limitations under the License.
  *
  */
+#include <cstddef>
+#include <memory>
+#include <mutex>
+#include <vector>
 
 #include <gtest/gtest.h>
 
@@ -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<std::mutex> lock(mutex);
+ magnetometerMsgs.push_back(_msg);
+ }
+
+ msgs::Magnetometer at(const size_t index) const
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+ return magnetometerMsgs.at(index);
+ }
+
+ size_t size() const
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+ return magnetometerMsgs.size();
+ }
+
+private:
+ mutable std::mutex mutex;
+ std::vector<msgs::Magnetometer> 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<transport::Node> gaussNode =
+ std::make_unique<transport::Node>();
+ 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<std::string>("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<transport::Node> teslaNode =
+ std::make_unique<transport::Node>();
+ 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());
+ }
+}

test/worlds/magnetometer_with_tesla.sdf

@@ -0,0 +1,94 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+ <world name="magnetometer_sensor">
+ <magnetic_field>0.94 0.76 -0.12</magnetic_field>
+ <physics name="1ms" type="ode">
+ <max_step_size>0.001</max_step_size>
+ <real_time_factor>0</real_time_factor>
+ </physics>
+ <plugin
+ filename="gz-sim-physics-system"
+ name="gz::sim::systems::Physics">
+ </plugin>
+ <plugin
+ filename="gz-sim-magnetometer-system"
+ name="gz::sim::systems::Magnetometer">
+ <use_tesla_for_magnetic_field>false</use_tesla_for_magnetic_field>
+ </plugin>
+ <plugin
+ filename="gz-sim-scene-broadcaster-system"
+ name="gz::sim::systems::SceneBroadcaster">
+ </plugin>
+
+ <!--We need spherical coordinates to update the magnetometer-->
+ <spherical_coordinates>
+ <surface_model>EARTH_WGS84</surface_model>
+ <world_frame_orientation>ENU</world_frame_orientation>
+ <latitude_deg>37.3861</latitude_deg>
+ <longitude_deg>-122.0839</longitude_deg>
+ <elevation>0</elevation>
+ <heading_deg>0</heading_deg>
+ </spherical_coordinates>
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <material>
+ <ambient>0.8 0.8 0.8 1</ambient>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="magnetometer_model">
+ <pose>4 0 3.0 0 0.0 3.14</pose>
+ <link name="link">
+ <pose>0.05 0.05 0.05 0 0 1.570796327</pose>
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.000166667</ixx>
+ <iyy>0.000166667</iyy>
+ <izz>0.000166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.1 0.1 0.1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.1 0.1 0.1</size>
+ </box>
+ </geometry>
+ </visual>
+ <sensor name="magnetometer_sensor" type="magnetometer">
+ <always_on>1</always_on>
+ <update_rate>1</update_rate>
+ </sensor>
+ </link>
+ </model>
+
+ </world>
+</sdf>