Skip to content

IMU custom_rpy tag parsing added #178

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

Merged
adityapande-1995 merged 8 commits into from
Dec 21, 2021
Merged

IMU custom_rpy tag parsing added #178

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
ff3df6f
Imu custom_rpy exposed via sdf, added test case
adityapande-1995 Dec 14, 2021
62e990f
Added username to TODO
adityapande-1995 Dec 14, 2021
b2d4167
Fixed linting errors
adityapande-1995 Dec 14, 2021
99fa6f0
sdf parsing minor fix
adityapande-1995 Dec 14, 2021
fa3799b
Added test case when parent_frame set to non empty string
adityapande-1995 Dec 17, 2021
c08f746
ignerr changed to ignwarn
adityapande-1995 Dec 17, 2021
7aa93b6
changed tabs to spaces
adityapande-1995 Dec 17, 2021
4848461
Set test verbosity to 3 to show ignwarn messages
scpeters Dec 21, 2021
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
12 changes: 12 additions & 0 deletions src/ImuSensor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,18 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf)
return false;
}

// Set orientation reference frame
// TODO(adityapande-1995) : Add support for named frames like
// ENU using ign-gazebo
if (_sdf.ImuSensor()->Localization() == "CUSTOM") {
if (_sdf.ImuSensor()->CustomRpyParentFrame() == "") {
this->SetOrientationReference(ignition::math::Quaterniond(
_sdf.ImuSensor()->CustomRpy()));
}
} else {
ignerr << "This tag is not yet supported" << std::endl;
Copy link
Member

@scpeters scpeters Dec 14, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure an error message is needed here, since ign-gazebo will handle any other types of <localization>.

You may consider a warning message if CustomRpyParentFrame() is not an empty string, since we don't currently support that use case.

Copy link
Contributor Author

@adityapande-1995 adityapande-1995 Dec 17, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done !

Copy link
Member

@scpeters scpeters Dec 21, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

thanks; I just pushed 4848461 to update the verbosity level used by the test so that the ignwarn message will show in the console output

Copy link
Contributor

@chapulina chapulina Jan 5, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

since ign-gazebo will handle any other types of

@scpeters , how did you envision this being implemented?

The IMU system in ign-gazebo should have knowledge of the current spherical coordinates, and it also should have access to sdf::Imu::Localization, so I guess it could call sensors::ImuSensor::SetOrientationReference as needed.

An alternative approach, if we want to keep most of the frame-conversion logic in ign-sensors, would be to expose an API like sensors::ImuSensor::SetWorldFrameOrientation so that the Gazebo system just lets the sensor know what the world is using, and the sensor performs all the calculations. I like this option better because it keeps all the SDF parsing within the sensor class.

Also worth noting that we don't support world frames other than ENU right now, so we should be able to default to that for a long time.

Copy link
Member

@scpeters scpeters Jan 7, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've added an example world with multiple IMU sensors with different Localization values in gazebosim/gz-sim@051f777

The IMU system in ign-gazebo should have knowledge of the current spherical coordinates, and it also should have access to sdf::Imu::Localization, so I guess it could call sensors::ImuSensor::SetOrientationReference as needed.

An alternative approach, if we want to keep most of the frame-conversion logic in ign-sensors, would be to expose an API like sensors::ImuSensor::SetWorldFrameOrientation so that the Gazebo system just lets the sensor know what the world is using, and the sensor performs all the calculations. I like this option better because it keeps all the SDF parsing within the sensor class.

can you describe the data types that you envision that sensors::ImuSensor::SetWorldFrameOrientation would use? I'm not sure exactly what you are proposing

Copy link
Contributor

@chapulina chapulina Jan 8, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

humm ok I see there's a use case there. Then I guess the heading needs to be passed to the IMU as well

Copy link
Member

@scpeters scpeters Jan 8, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

in that case does sensors::ImuSensor::SetWorldFrameOrientation(const Quaterniond &_rot, const WorldFrameEnumType _relativeTo = ENU); seem like a reasonable API?

Copy link
Contributor

@chapulina chapulina Jan 8, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So in that case, ign-gazebo would populate _rot with just the heading?

Copy link
Member

@scpeters scpeters Jan 8, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yeah, it could call SetWorldFrameOrientation(Quaterniond(0, 0, heading), ENU);

Copy link
Contributor

@chapulina chapulina Jan 8, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Cool, that sounds reasonable to me 👍

}

if (this->Topic().empty())
this->SetTopic("/imu");

Expand Down
54 changes: 54 additions & 0 deletions src/ImuSensor_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -390,6 +390,60 @@ TEST(ImuSensor_TEST, Orientation)

}

//////////////////////////////////////////////////
TEST(ImuSensor_TEST, OrientationReference)
{
// Create a sensor manager
ignition::sensors::Manager mgr;

sdf::ElementPtr imuSDF;

std::ostringstream stream;
stream
<< "<?xml version='1.0'?>"
<< "<sdf version='1.6'>"
<< " <model name='m1'>"
<< " <link name='link1'>"
<< " <sensor name='imu_sensor' type='imu'>"
<< " <topic>imu_test</topic>"
<< " <update_rate>1</update_rate>"
<< " <imu>"
<< " <orientation_reference_frame>"
<< " <localization>CUSTOM</localization>"
<< " <custom_rpy>1.570795 0 0</custom_rpy>"
<< " </orientation_reference_frame>"
<< " </imu>"
<< " <always_on>1</always_on>"
<< " <visualize>true</visualize>"
<< " </sensor>"
<< " </link>"
<< " </model>"
<< "</sdf>";

sdf::SDFPtr sdfParsed(new sdf::SDF());
sdf::init(sdfParsed);
if (!sdf::readString(stream.str(), sdfParsed)) {
imuSDF = sdf::ElementPtr();
} else {
imuSDF = sdfParsed->Root()->GetElement("model")->GetElement("link")
->GetElement("sensor");
}

// Create an ImuSensor
auto sensor = mgr.CreateSensor<ignition::sensors::ImuSensor>(
imuSDF);

// Make sure the above dynamic cast worked.
ASSERT_NE(nullptr, sensor);

sensor->Update(std::chrono::steady_clock::duration(
std::chrono::nanoseconds(10000000)));

math::Quaterniond orientValue(math::Vector3d(-1.570795, 0, 0));
EXPECT_EQ(orientValue, sensor->Orientation());

}

//////////////////////////////////////////////////
int main(int argc, char **argv)
{
Expand Down