Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Supply world frame orientation and heading to IMU sensor : DCO fix #1427

Merged
merged 2 commits into from
Apr 6, 2022
Merged

Supply world frame orientation and heading to IMU sensor : DCO fix #1427

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0b10adb
Ported approved changes from https://github.com/ignitionrobotics/ign-…
adityapande-1995 Apr 5, 2022
60ced56
Merge branch 'ign-gazebo6' into aditya/named_frames_imu_system_dco_fix
hidmic Apr 6, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 23 additions & 0 deletions src/systems/imu/Imu.cc
View file
Open in desktop
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
Open in desktop
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