IMU `custom_rpy` tag parsing added #178

adityapande-1995 · 2021-12-14T01:12:54Z

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

🎉 New feature

Closes #174

Summary

According to REP 145, an IMU can report orientation with respect to the world, or some given reference frame. The method SetOrientationReference enables this, but this cannot be set via sdf currently.
The orientation_reference_frame tag ( http://sdformat.org/spec?ver=1.9&elem=sensor#imu_orientation_reference_frame) exposes this, but it is ignored currently in the Load method for the IMU sensor. This PR aims to set reference orientation when the localization tag is set to CUSTOM and custom_rpy is set to some non zero angles.

Test it

I've added a test case, but if one wants to run it independently, you can use the following sdf snippet :

<?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>

After first update, if the IMU orientation is (0 0 0) by default, it should be now (-1.570795 0 0) as the reference axis has been modified in the sdf.

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
codecheck passed (See contributing)
All tests passed (See test coverage)
While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

Signed-off-by: Aditya <[email protected]>

src/ImuSensor.cc

Signed-off-by: Aditya <[email protected]>

codecov · 2021-12-14T21:43:10Z

Codecov Report

Merging #178 (4848461) into ign-sensors6 (9c075d0) will increase coverage by 0.05%.
The diff coverage is 100.00%.

@@               Coverage Diff                @@
##           ign-sensors6     #178      +/-   ##
================================================
+ Coverage         75.96%   76.01%   +0.05%     
================================================
  Files                27       27              
  Lines              2754     2760       +6     
================================================
+ Hits               2092     2098       +6     
  Misses              662      662

Impacted Files	Coverage Δ
src/ImuSensor.cc	`91.30% <100.00%> (+0.47%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 9c075d0...4848461. Read the comment docs.

scpeters

can you add another test case with a non-empty custom RPY parent frame?

scpeters · 2021-12-14T23:08:13Z

src/ImuSensor.cc

+        _sdf.ImuSensor()->CustomRpy()));
+    }
+  } else {
+    ignerr << "This tag is not yet supported" << std::endl;


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.

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

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.

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

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

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

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

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

Cool, that sounds reasonable to me 👍

src/ImuSensor.cc

Signed-off-by: Aditya <[email protected]>

Signed-off-by: Steve Peters <[email protected]>

scpeters · 2021-12-21T00:44:03Z

looks good! don't forget to edit the commit message when squashing

osrf-triage · 2022-01-10T21:45:52Z

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/new-ignition-releases-2022-01-10/1228/1

Imu custom_rpy exposed via sdf, added test case

ff3df6f

Signed-off-by: Aditya <[email protected]>

adityapande-1995 requested a review from scpeters December 14, 2021 01:12

adityapande-1995 requested a review from iche033 as a code owner December 14, 2021 01:12

github-actions bot added the 🏯 fortress Ignition Fortress label Dec 14, 2021

adityapande-1995 changed the title ~~Imu custom_rpy exposed via sdf, added test case~~ Imu custom_rpy exposed via sdf Dec 14, 2021

Added username to TODO

62e990f

Signed-off-by: Aditya <[email protected]>

adityapande-1995 changed the title ~~Imu custom_rpy exposed via sdf~~ IMU custom_rpy tag parsing added Dec 14, 2021

scpeters reviewed Dec 14, 2021

View reviewed changes

src/ImuSensor.cc Outdated Show resolved Hide resolved

adityapande-1995 added 2 commits December 14, 2021 12:49

Fixed linting errors

b2d4167

Signed-off-by: Aditya <[email protected]>

sdf parsing minor fix

99fa6f0

Signed-off-by: Aditya <[email protected]>

scpeters reviewed Dec 14, 2021

View reviewed changes

adityapande-1995 and others added 4 commits December 17, 2021 13:47

Added test case when parent_frame set to non empty string

fa3799b

Signed-off-by: Aditya <[email protected]>

ignerr changed to ignwarn

c08f746

Signed-off-by: Aditya <[email protected]>

changed tabs to spaces

7aa93b6

Signed-off-by: Aditya <[email protected]>

Set test verbosity to 3 to show ignwarn messages

4848461

Signed-off-by: Steve Peters <[email protected]>

scpeters approved these changes Dec 21, 2021

View reviewed changes

adityapande-1995 merged commit 8ae7167 into ign-sensors6 Dec 21, 2021

adityapande-1995 deleted the aditya/imu_ref_orintation branch December 21, 2021 18:37

adityapande-1995 mentioned this pull request Jan 11, 2022

IMU sensor API to get world ref frame and heading offset #186

Merged

8 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

IMU `custom_rpy` tag parsing added #178

IMU `custom_rpy` tag parsing added #178

adityapande-1995 commented Dec 14, 2021 •

edited

Loading

codecov bot commented Dec 14, 2021 •

edited

Loading

scpeters left a comment

scpeters Dec 14, 2021

adityapande-1995 Dec 17, 2021

scpeters Dec 21, 2021

chapulina Jan 5, 2022

scpeters Jan 7, 2022

chapulina Jan 8, 2022

scpeters Jan 8, 2022

chapulina Jan 8, 2022

scpeters Jan 8, 2022

chapulina Jan 8, 2022

scpeters commented Dec 21, 2021

osrf-triage commented Jan 10, 2022

IMU custom_rpy tag parsing added #178

IMU custom_rpy tag parsing added #178

Conversation

adityapande-1995 commented Dec 14, 2021 • edited Loading

🎉 New feature

Summary

Test it

Checklist

codecov bot commented Dec 14, 2021 • edited Loading

Codecov Report

scpeters left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

scpeters commented Dec 21, 2021

osrf-triage commented Jan 10, 2022

IMU `custom_rpy` tag parsing added #178

IMU `custom_rpy` tag parsing added #178

adityapande-1995 commented Dec 14, 2021 •

edited

Loading

codecov bot commented Dec 14, 2021 •

edited

Loading