Skip to content

Commit

Permalink
Supply world frame orientation and heading to IMU sensor
Browse files Browse the repository at this point in the history 
Signed-off-by: Aditya <aditya050995@gmail.com>

Co-authored-by: Michel Hidalgo <michel@ekumenlabs.com>
  • Loading branch information
@adityapande-1995 @hidmic
adityapande-1995 and hidmic authored Apr 6, 2022
1 parent 031e334 commit 2e8892a
Show file tree
Hide file tree
Showing 5 changed files with 772 additions and 1 deletion.
23 changes: 23 additions & 0 deletions src/systems/imu/Imu.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include <ignition/sensors/SensorFactory.hh>
#include <ignition/sensors/ImuSensor.hh>

#include "ignition/gazebo/World.hh"
#include "ignition/gazebo/components/AngularVelocity.hh"
#include "ignition/gazebo/components/Imu.hh"
#include "ignition/gazebo/components/Gravity.hh"
Expand Down Expand Up @@ -206,6 +207,28 @@ void ImuPrivate::AddSensor(
math::Pose3d p = worldPose(_entity, _ecm);
sensor->SetOrientationReference(p.Rot());

// Get world frame orientation and heading.
// If <orientation_reference_frame> includes a named
// frame like NED, that must be supplied to the IMU sensor,
// otherwise orientations are reported w.r.t to the initial
// orientation.
if (data.Element()->HasElement("imu")) {
auto imuElementPtr = data.Element()->GetElement("imu");
if (imuElementPtr->HasElement("orientation_reference_frame")) {
double heading = 0.0;

ignition::gazebo::World world(worldEntity);
if (world.SphericalCoordinates(_ecm))
{
auto sphericalCoordinates = world.SphericalCoordinates(_ecm).value();
heading = sphericalCoordinates.HeadingOffset().Radian();
}

sensor->SetWorldFrameOrientation(math::Quaterniond(0, 0, heading),
ignition::sensors::WorldFrameEnumType::ENU);
}
}

// Set whether orientation is enabled
if (data.ImuSensor())
{
Expand Down
329 changes: 328 additions & 1 deletion test/integration/imu_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,51 @@ class ImuTest : public InternalFixture<::testing::Test>

std::mutex mutex;
std::vector<msgs::IMU> imuMsgs;
msgs::IMU lastImuMsgENU;
msgs::IMU lastImuMsgNED;
msgs::IMU lastImuMsgNWU;
msgs::IMU lastImuMsgCUSTOM;
msgs::IMU lastImuMsgDEFAULT;

/////////////////////////////////////////////////
void imuENUCb(const msgs::IMU &_msg)
{
lastImuMsgENU = _msg;
}

/////////////////////////////////////////////////
void imuNEDCb(const msgs::IMU &_msg)
{
lastImuMsgNED = _msg;
}

/////////////////////////////////////////////////
void imuNWUCb(const msgs::IMU &_msg)
{
lastImuMsgNWU = _msg;
}

/////////////////////////////////////////////////
void imuCUSTOMCb(const msgs::IMU &_msg)
{
lastImuMsgCUSTOM = _msg;
}

/////////////////////////////////////////////////
void imuDEFULTCb(const msgs::IMU &_msg)
{
lastImuMsgDEFAULT = _msg;
}

/////////////////////////////////////////////////
void clearLastImuMsgs()
{
lastImuMsgCUSTOM.Clear();
lastImuMsgENU.Clear();
lastImuMsgNED.Clear();
lastImuMsgNWU.Clear();
lastImuMsgDEFAULT.Clear();
}

/////////////////////////////////////////////////
void imuCb(const msgs::IMU &_msg)
Expand Down Expand Up @@ -214,7 +259,7 @@ TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelFalling))
}

/////////////////////////////////////////////////
// The test checks to make sure orientation is not published if it is deabled
// The test checks to make sure orientation is not published if it is disabled
TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OrientationDisabled))
{
imuMsgs.clear();
Expand Down Expand Up @@ -250,3 +295,285 @@ TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OrientationDisabled))
}
mutex.unlock();
}

/////////////////////////////////////////////////
// The test checks if the orientation is published according to the
// localization tag
TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(NamedFrames))
{
imuMsgs.clear();
clearLastImuMsgs();

// Start server
ServerConfig serverConfig;
const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "imu_named_frame.sdf");
serverConfig.SetSdfFile(sdfFile);

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

auto topicENU = "/imu_test_ENU";
auto topicNED = "/imu_test_NED";
auto topicNWU = "/imu_test_NWU";
auto topicCUSTOM = "/imu_test_CUSTOM";
auto topicDEFAULT = "/imu_test_DEFAULT";

// subscribe to imu topic
transport::Node node;
node.Subscribe(topicENU, &imuENUCb);
node.Subscribe(topicNED, &imuNEDCb);
node.Subscribe(topicNWU, &imuNWUCb);
node.Subscribe(topicCUSTOM, &imuCUSTOMCb);
node.Subscribe(topicDEFAULT, &imuDEFULTCb);

// Run server
server.Run(true, 200u, false);

// Check we received messages
EXPECT_TRUE(lastImuMsgENU.has_orientation());
EXPECT_TRUE(lastImuMsgNED.has_orientation());
EXPECT_TRUE(lastImuMsgNWU.has_orientation());
EXPECT_TRUE(lastImuMsgCUSTOM.has_orientation());
EXPECT_TRUE(lastImuMsgDEFAULT.has_orientation());

// For the DEFAULT msg, orientation is reported relative
// to the original pose, which should be identity quaternion.
EXPECT_NEAR(lastImuMsgDEFAULT.orientation().x(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgDEFAULT.orientation().y(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgDEFAULT.orientation().z(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgDEFAULT.orientation().w(), 1, 1e-2);

// For the ENU msg
EXPECT_NEAR(lastImuMsgENU.orientation().x(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgENU.orientation().y(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgENU.orientation().z(), 1, 1e-2);
EXPECT_NEAR(lastImuMsgENU.orientation().w(), 0, 1e-2);

// For the NED msg
EXPECT_NEAR(lastImuMsgNED.orientation().x(), -0.707, 1e-2);
EXPECT_NEAR(lastImuMsgNED.orientation().y(), 0.707, 1e-2);
EXPECT_NEAR(lastImuMsgNED.orientation().z(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgNED.orientation().w(), 0, 1e-2);

// For the NWU msg
EXPECT_NEAR(lastImuMsgNWU.orientation().x(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgNWU.orientation().y(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgNWU.orientation().z(), 0.707, 1e-2);
EXPECT_NEAR(lastImuMsgNWU.orientation().w(), 0.707, 1e-2);

// For the CUSTOM msg
EXPECT_NEAR(lastImuMsgCUSTOM.orientation().x(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgCUSTOM.orientation().y(), 0.707, 1e-2);
EXPECT_NEAR(lastImuMsgCUSTOM.orientation().z(), 0.707, 1e-2);
EXPECT_NEAR(lastImuMsgCUSTOM.orientation().w(), 0, 1e-2);
}

/////////////////////////////////////////////////
// The test checks if the orientation is published according to the
// localization tag, with heading_deg also accounted for
TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(NamedFramesWithHeading))
{
imuMsgs.clear();
clearLastImuMsgs();

// Start server
ServerConfig serverConfig;
const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "imu_heading_deg.sdf");
serverConfig.SetSdfFile(sdfFile);

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

auto topicENU = "/imu_test_ENU";
auto topicNED = "/imu_test_NED";
auto topicNWU = "/imu_test_NWU";
auto topicCUSTOM = "/imu_test_CUSTOM";
auto topicDEFAULT = "/imu_test_DEFAULT";

// subscribe to imu topic
transport::Node node;
node.Subscribe(topicENU, &imuENUCb);
node.Subscribe(topicNED, &imuNEDCb);
node.Subscribe(topicNWU, &imuNWUCb);
node.Subscribe(topicCUSTOM, &imuCUSTOMCb);
node.Subscribe(topicDEFAULT, &imuDEFULTCb);

// Run server
server.Run(true, 200u, false);

// Check we received messages
EXPECT_TRUE(lastImuMsgENU.has_orientation());
EXPECT_TRUE(lastImuMsgNED.has_orientation());
EXPECT_TRUE(lastImuMsgNWU.has_orientation());
EXPECT_TRUE(lastImuMsgCUSTOM.has_orientation());
EXPECT_TRUE(lastImuMsgDEFAULT.has_orientation());

// For the DEFAULT msg, orientation is reported relative
// to the original pose, which should be identity quaternion.
EXPECT_NEAR(lastImuMsgDEFAULT.orientation().x(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgDEFAULT.orientation().y(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgDEFAULT.orientation().z(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgDEFAULT.orientation().w(), 1, 1e-2);

// For the ENU msg
EXPECT_NEAR(lastImuMsgENU.orientation().x(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgENU.orientation().y(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgENU.orientation().z(), 0.707, 1e-2);
EXPECT_NEAR(lastImuMsgENU.orientation().w(), 0.707, 1e-2);

// For the NED msg
EXPECT_NEAR(lastImuMsgNED.orientation().x(), -1, 1e-2);
EXPECT_NEAR(lastImuMsgNED.orientation().y(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgNED.orientation().z(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgNED.orientation().w(), 0, 1e-2);

// For the NWU msg
EXPECT_NEAR(lastImuMsgNWU.orientation().x(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgNWU.orientation().y(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgNWU.orientation().z(), 0, 1e-2);
EXPECT_NEAR(lastImuMsgNWU.orientation().w(), 1, 1e-2);

// For the CUSTOM msg
EXPECT_NEAR(lastImuMsgCUSTOM.orientation().x(), -0.5, 1e-2);
EXPECT_NEAR(lastImuMsgCUSTOM.orientation().y(), 0.5, 1e-2);
EXPECT_NEAR(lastImuMsgCUSTOM.orientation().z(), 0.5, 1e-2);
EXPECT_NEAR(lastImuMsgCUSTOM.orientation().w(), 0.5, 1e-2);
}

/////////////////////////////////////////////////
// The test checks if orientations are reported correctly for a rotating body.
// The world includes a sphere rolling down a plane, with axis of rotation
// as the "west" direction vector, using the right hand rule.
TEST_F(ImuTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RotatingBody))
{
imuMsgs.clear();
clearLastImuMsgs();

// Start server
ServerConfig serverConfig;
const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "imu_rotating_demo.sdf");
serverConfig.SetSdfFile(sdfFile);

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

msgs::IMU currentImuMsgDefault_1;
msgs::IMU currentImuMsgDefault_2;
msgs::IMU currentImuMsgDefault_3;

std::function<void(const msgs::IMU &_msg)> defaultCb1 =
[&](const msgs::IMU &_msg)
{
currentImuMsgDefault_1 = _msg;
};
std::function<void(const msgs::IMU &_msg)> defaultCb2 =
[&](const msgs::IMU &_msg)
{
currentImuMsgDefault_2 = _msg;
};
std::function<void(const msgs::IMU &_msg)> defaultCb3 =
[&](const msgs::IMU &_msg)
{
currentImuMsgDefault_3 = _msg;
};

// subscribe to imu topic
transport::Node node;
node.Subscribe("/imu_test_ENU", &imuENUCb);
node.Subscribe("/imu_test_NED", &imuNEDCb);
node.Subscribe("/imu_test_DEFAULT_1", defaultCb1);
node.Subscribe("/imu_test_DEFAULT_2", defaultCb2);
node.Subscribe("/imu_test_DEFAULT_3", defaultCb3);

// Run server
server.Run(true, 50u, false);

// Store initial orientations reported by the IMUs
auto initialOrientationDEFAULT_1 = msgs::Convert(
currentImuMsgDefault_1.orientation());
auto initialOrientationDEFAULT_2 = msgs::Convert(
currentImuMsgDefault_2.orientation());
auto initialOrientationDEFAULT_3 = msgs::Convert(
currentImuMsgDefault_3.orientation());
auto initialOrientationENU = msgs::Convert(
lastImuMsgENU.orientation());
auto initialOrientationNED = msgs::Convert(
lastImuMsgNED.orientation());

server.Run(true, 1500u, false);

// Store final orientations reported by the IMUs
auto finalOrientationDEFAULT_1 = msgs::Convert(
currentImuMsgDefault_1.orientation());
auto finalOrientationDEFAULT_2 = msgs::Convert(
currentImuMsgDefault_2.orientation());
auto finalOrientationDEFAULT_3 = msgs::Convert(
currentImuMsgDefault_3.orientation());
auto finalOrientationENU = msgs::Convert(
lastImuMsgENU.orientation());
auto finalOrientationNED = msgs::Convert(
lastImuMsgNED.orientation());

auto differenceOrientationDEFAULT_1 = finalOrientationDEFAULT_1 *
initialOrientationDEFAULT_1.Inverse();
auto differenceOrientationDEFAULT_2 = finalOrientationDEFAULT_2 *
initialOrientationDEFAULT_2.Inverse();
auto differenceOrientationDEFAULT_3 = finalOrientationDEFAULT_3 *
initialOrientationDEFAULT_3.Inverse();

auto differenceOrientationENU = finalOrientationENU *
initialOrientationENU.Inverse();
auto differenceOrientationNED = finalOrientationNED *
initialOrientationNED.Inverse();

// Since the sphere has rotated along the west direction,
// pitch and yaw change for ENU reporting IMU should be zero,
// and roll should have some non trivial negative value.
EXPECT_TRUE((differenceOrientationENU.Roll() < -0.04));
EXPECT_NEAR(differenceOrientationENU.Pitch(), 0, 1e-2);
EXPECT_NEAR(differenceOrientationENU.Yaw(), 0, 1e-2);

// Similarly, roll and yaw for NED reporting IMU should be zero,
// and pitch should be some non trivial negative value.
EXPECT_NEAR(differenceOrientationNED.Roll(), 0, 1e-2);
EXPECT_TRUE((differenceOrientationNED.Pitch() < -0.04));
EXPECT_NEAR(differenceOrientationNED.Yaw(), 0, 1e-2);

// In the sdf world, the IMU model & link have a yaw pose of PI/2,
// which means the initial orientation of DEFAULT IMU is
// effectively WND (by rotating ENU by PI about N). Therefore,
// pitch and yaw for DEFAULT case should be zero, and roll
// should be nontrivial positive value.
EXPECT_TRUE((differenceOrientationDEFAULT_1.Roll() > 0.04));
EXPECT_NEAR(differenceOrientationDEFAULT_1.Pitch(), 0, 1e-2);
EXPECT_NEAR(differenceOrientationDEFAULT_1.Yaw(), 0, 1e-2);

// For DEFAULT_2, model has a pose PI/2 0 0 & link has a pose of
// 0 PI/2 0, which makes the frame NUE.
EXPECT_NEAR(differenceOrientationDEFAULT_2.Roll(), 0, 1e-2);
EXPECT_NEAR(differenceOrientationDEFAULT_2.Pitch(), 0, 1e-2);
EXPECT_TRUE((differenceOrientationDEFAULT_2.Yaw() < -0.04));

// For DEFAULT_3, model has a pose PI/2 0 0 & link has a pose of
// 0 0 PI/2, which makes the frame UWS.
EXPECT_NEAR(differenceOrientationDEFAULT_3.Roll(), 0, 1e-2);
EXPECT_TRUE((differenceOrientationDEFAULT_3.Pitch() > 0.04));
EXPECT_NEAR(differenceOrientationDEFAULT_3.Yaw(), 0, 1e-2);

// Those nontrivial values should match for all sensors.
EXPECT_NEAR(differenceOrientationENU.Roll(),
differenceOrientationNED.Pitch(), 1e-4);

EXPECT_NEAR(differenceOrientationENU.Roll(),
-differenceOrientationDEFAULT_1.Roll(), 1e-4);
EXPECT_NEAR(differenceOrientationENU.Roll(),
differenceOrientationDEFAULT_2.Yaw(), 1e-4);
EXPECT_NEAR(differenceOrientationENU.Roll(),
-differenceOrientationDEFAULT_3.Pitch(), 1e-4);
}
Loading

0 comments on commit 2e8892a

Please sign in to comment.