Support spherical coordinates

[chapulina]

The SDFormat spec provides the //world/spherical_coordinates element which allows defining latitude and longitude for the origin of a simulated world. We should support that in Ignition Gazebo.

Desired behavior

It should be possible to query the pose of any entity in the world in spherical coordinates, using as reference the coordinates set at startup. For example, for this world:

  <world name="flat_earth">

    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <world_frame_orientation>ENU</world_frame_orientation>
      <latitude_deg>-22.9</latitude_deg>
      <longitude_deg>-43.2</longitude_deg>
      <elevation>0</elevation>
    </spherical_coordinates>

    <model name="robot">
      <pose>0 0 0.0 0 0 0</pose>
      ...
    </model>

  </world>

Developers should be able to query:

auto latLonElev = sphericalCoords(_robotEntity, _ecm);
EXPECT_EQ(latLonElev.X(), -22.9);
EXPECT_EQ(latLonElev.Y(), -43.2);
EXPECT_EQ(latLonElev.Z(), 0);

Alternatives considered

We could choose not to include spherical coordinates as part of the core simulator and leave it up to each plugin to figure it out. But since the SDF spec supports it and several plugins can benefit from it, I think it's worth it providing core functionality for it.

Implementation suggestion

Gazebo Classic supports that through a gazebo::common::SphericalCoordinates object belonging to gazebo::physics::World, which is populated here. All other classes that want to make use of spherical coordinates can query that.

That class has been ported to Ignition Math: ignition::math::SphericalCoordinates, which we can use here.

I'd suggest:

1. Parse <spherical_coordinates> into sdf::World
2. Create a component::SphericalCoordinates that wraps ignition::math::SphericalCoordinates
3. Add that component to the World entity when parsing the SDF
4. Provide a helper function in Util.hh that returns the spherical coordinates for any entity according to its pose and the world's component

Additional context

We should make sure we don't inherit issue gazebosim/gazebo-classic#2022 from Gazebo classic. We may also want to address gazebosim/gz-math#51.

[chapulina]

Other nice-to-have features:

• Updating the world origin coordinates via an ign-transport service
• Support for spherical coordinates when inserting new models through the /create service
• Support for spherical coordinates when moving models through the /set_pose (or equivalent) service
• Displaying spherical coordinates on the component inspector

[chapulina]

I started working on this issue and I'm trying to understand how the SDF element <spherical_coordinates><world_frame_orientation> is meant to be used.

I tracked down some background on that element that I'll summarize here for future reference:

I believe that tag was added to the SDFormat spec in the context of gazebosim/gazebo-classic#1959:

• SDFormat PR (274) was closed and reopened as PR 284, which was merged.

• That added the following to the SDF spec:

  • <imu><orientation_reference_frame>
  • <spherical_coordinates><world_frame_orientation>

• The merged PR points to branch issue_1959_gazebo7 on Gazebo classic as the corresponding changes to Gazebo, but it looks like that was never merged.

• Also, searching for orientation_reference_frame or world_frame_orientation on Gazebo 11 gives no results. So I think it's safe to say that we don't have a working reference implementation for how world_frame_orientation should be used.

I'm still looking into it; I may either ignore that element for now (and forever?), or port part of that Gazebo 11 branch to Ignition Math.

[scpeters]

you're right, it looks like we never started using the <world_frame_orientation /> tag in gazebo, though it's not only relevant to IMU's; it would also affect the output of gazebo's GPS sensor. To be honest, our SphericalCoordiantes class is very rudimentary. If there was a way to use proj, I think that would be the most powerful approach. Maybe @mjcarroll has an opinion based on his experience with NavSat hardware?

[chapulina]

Thanks for the input, @scpeters . I looked a bit at proj and it seems to be pretty powerful. I think we definitely should leave the architecture flexible in case we want to integrate it later, but for now, I was thinking of keeping it simple and using the class that we already have. My current idea:

1. Fix the most pressing bug with SphericalCoordinates, see 🌐 Spherical coordinates: bug fix, docs and sanity checks gz-math#235
2. Add ignition::math::SphericalCoordinates sdf::World::SphericalCoordinates, ignoring <world_frame_orientation> (🌐 Parse spherical coordinates sdformat#685)
3. Add a gazebo::component::SphericalCoordinates component that wraps math::SphericalCoordinates
4. Provide the utility function and services described above.

In the future, if we need more precision than what Ignition Math is providing and want to integrate proj, we can create a system plugin that reads / writes the reference lat / lon / elevation / heading to/from gazebo::component::SphericalCoordinates and offers alternative transport services. In this scenario, math::SphericalCoordinates is just used to hold the data but not for computation.

Let me know what you think of this 2-step approach.