Skip to content

Commit

Permalink
Add magnetometer plugin parameter to publish units as tesla
Browse files Browse the repository at this point in the history
  • Loading branch information
@gilborty
gilborty committed Mar 17, 2024
1 parent 6cfcd07 commit 5d47e3e
Show file tree
Hide file tree
Showing 4 changed files with 281 additions and 1 deletion.
32 changes: 31 additions & 1 deletion src/systems/magnetometer/Magnetometer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
}
};

//////////////////////////////////////////////////
Expand All @@ -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)
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
Expand Down Expand Up @@ -494,6 +523,7 @@ void MagnetometerPrivate::RemoveMagnetometerEntities(
}

GZ_ADD_PLUGIN(Magnetometer, System,
Magnetometer::ISystemConfigure,
Magnetometer::ISystemPreUpdate,
Magnetometer::ISystemPostUpdate
)
Expand Down
7 changes: 7 additions & 0 deletions src/systems/magnetometer/Magnetometer.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ namespace systems
/// current location.
class Magnetometer:
public System,
public ISystemConfigure,
public ISystemPreUpdate,
public ISystemPostUpdate
{
Expand All @@ -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;
Expand Down
149 changes: 149 additions & 0 deletions test/integration/magnetometer_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,3 +141,152 @@ 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);
}

msgs::Magnetometer latest() const
{
std::lock_guard<std::mutex> lock(mutex);
return magnetometerMsgs.back();
}

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 = 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());
}
}
94 changes: 94 additions & 0 deletions test/worlds/magnetometer_with_tesla.sdf
View file
Original file line number Diff line number Diff line change
@@ -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>

0 comments on commit 5d47e3e

Please sign in to comment.