IMU sensor API to get world ref frame and heading offset

[adityapande-1995]

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

🎉 New feature

Summary

As per the discussion here : #178 (comment), this PR aims to expose SetWorldFrameOrientation API which accepts heading offset and world frame orientation (ENU by default). If the <localization> tag for IMU sensor is set to any named frame (https://github.com/ignitionrobotics/sdformat/blob/6404afdf959982f759526dea8700037b50fa7758/sdf/1.9/imu.sdf#L7-L30), the sensor should output its orientation with respect to the frame specified in the <localization> tag.

There would be another follow up PR in IMU sensor system to use this API

Test it

Added some test cases to ImuSensor_TEST.cc

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 and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

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

[codecov]

Codecov Report

❗ No coverage uploaded for pull request base (ign-sensors6@2614d1d). Click here to learn what that means.
The diff coverage is n/a.

@@               Coverage Diff               @@
##             ign-sensors6     #186   +/-   ##
===============================================
  Coverage                ?   76.44%           
===============================================
  Files                   ?       29           
  Lines                   ?     2908           
  Branches                ?        0           
===============================================
  Hits                    ?     2223           
  Misses                  ?      685           
  Partials                ?        0           

---

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 2614d1d...744d0d1. Read the comment docs.

[adityapande-1995]

TODO

• custom_rpy logic cannot remain in the Load function in ImuSensor.cc as originally added here : IMU custom_rpy tag parsing added #178. The IMU system would override anything specified in the custom_rpy tag as of now : https://github.com/ignitionrobotics/ign-gazebo/blob/ae36d833f01e7078ff8bb397bee5981a0e93ed13/src/systems/imu/Imu.cc#L187. ✔️ (DONE)
• Verify that transformation quaternions used in the unit tests for named frames are correct.
• Open a PR in ign-gazebo to use these APIs and add an integration test.

[adityapande-1995]

Moved localization tag parsing logic to a separate method. This way the code in IMU system can be changed as follows :

void ImuPrivate::addIMU( ---- )
{
  // Get the world acceleration (defined in world frame)
  auto gravity = _ecm.Component<components::Gravity>(worldEntity);
  -------

  // create sensor
  std::string sensorScopedName =
      removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
  sdf::Sensor data = _imu->Data();
  data.SetName(sensorScopedName);
  // check topic
  -----

  std::unique_ptr<sensors::ImuSensor> sensor =
      this->sensorFactory.CreateSensor<
      sensors::ImuSensor>(data);
  if (nullptr == sensor)
  { ---- }

  // set sensor parent
 -----

  // set gravity
  ------

  // Get initial pose of sensor and set the reference z pos
  // The WorldPose component was just created and so it's empty
  // We'll compute the world pose manually here
  math::Pose3d p = worldPose(_entity, _ecm);
  sensor->SetOrientationReference(p.Rot());

  // Set topic
 ----

  // Set whether orientation is enabled
  if (data.ImuSensor())
  {
    sensor->SetOrientationEnabled(
        data.ImuSensor()->OrientationEnabled());
  }

  this->entitySensorMap.insert(
      std::make_pair(_entity, std::move(sensor)));
 
 // ------- ADDITION --------
 sensor->ReadLocalizationTag(data);
 sensor->SetWorldFrameOrientation(Quaterniond(0,0,0), WorldFrameType::ENU);
}

[scpeters]

I have some quick comments about the variable names which I think will clarify what the variables mean

[scpeters]

I'm trying to reproduce the error you are seeing

[scpeters]

Blocked due to an issue in integration with the IMU system.

let me know if you need help debugging this

[adityapande-1995]

Relevant PR in IMU system : gazebosim/gz-sim#1320

[scpeters]

I'm still looking through the tests, but I have a few comments for now

[scpeters]

why is this necessary?

[adityapande-1995]

Currently, SetWorldFrameOrientation is also responsible for applying the "CUSTOM" tag. I could separate this logic in a separate method though.

[scpeters]

why is this necessary?

[adityapande-1995]

Same reason above.

[scpeters]

ok, two more tiny nits, and then I'll be ready to approve

[osrf-triage]

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

https://community.gazebosim.org/t/new-ignition-releases-2022-04-13-fortress-edifice/1367/1