🌐 Parse spherical coordinates

include/sdf/World.hh

@@ -17,7 +17,9 @@
 #ifndef SDF_WORLD_HH_
 #define SDF_WORLD_HH_
 
+#include <optional>
 #include <string>
+#include <ignition/math/SphericalCoordinates.hh>
 #include <ignition/math/Vector3.hh>
 #include <ignition/utils/ImplPtr.hh>
 
@@ -136,6 +138,16 @@ namespace sdf
     /// \sa SphericalCoordinates
     public: void SetMagneticField(const ignition::math::Vector3d &_mag);
 
+    /// \brief Get the spherical coordinates for the world origin.
+    /// \return Spherical coordinates or null if not defined.
+    public: const ignition::math::SphericalCoordinates *
+        SphericalCoordinates() const;
+
+    /// \brief Set the spherical coordinates for the world origin.
+    /// \param[in] _coord The new coordinates for the world origin.
+    public: void SetSphericalCoordinates(
+        const ignition::math::SphericalCoordinates &_coord);
+
     /// \brief Get the number of models that are immediate (not nested) children
     /// of this World object.
     /// \remark ModelByName() can find nested models that are not immediate

sdf/1.9/spherical_coordinates.sdf

@@ -12,15 +12,9 @@
     <description>
       This field identifies how Gazebo world frame is aligned in Geographical
       sense.  The final Gazebo world frame orientation is obtained by rotating
-      a frame aligned with following notation by the field heading_deg (Note
-      that heading_deg corresponds to positive yaw rotation in the NED frame,
-      so it's inverse specifies positive Z-rotation in ENU or NWU).
+      a frame aligned with following notation by the field heading_deg.
       Options are:
         - ENU (East-North-Up)
-        - NED (North-East-Down)
-        - NWU (North-West-Up)
-      For example, world frame specified by setting world_orientation="ENU"
-      and heading_deg=-90° is effectively equivalent to NWU with heading of 0°.
     </description>
   </element>
   <element name="latitude_deg" type="double" default="0.0" required="1">
@@ -46,12 +40,10 @@
   <element name="heading_deg" type="double" default="0.0" required="1">
     <description>
       Heading offset of gazebo reference frame, measured as angle between
-      Gazebo world frame and the world_frame_orientation type (ENU/NED/NWU).
-      Rotations about the downward-vector (e.g. North to East) are positive.
-      The direction of rotation is chosen to be consistent with compass
-      heading convention (e.g. 0 degrees points North and 90 degrees
-      points East, positive rotation indicates counterclockwise rotation
-      when viewed from top-down direction).
+      Gazebo world frame and the world_frame_orientation type.
+      The direction of rotation follows the right-hand rule, so a positive
+      angle indicates clockwise rotation when viewed from top-down. Note
+      that this is not consistent with compass heading convention.
       The angle is specified in degrees.
     </description>
   </element>

src/World.cc

@@ -39,6 +39,11 @@ using namespace sdf;
 
 class sdf::World::Implementation
 {
+  /// \brief Populate sphericalCoordinates
+  /// \param[in] _elem `<spherical_coordinates>` element
+  /// \return Errors, if any.
+  public: Errors LoadSphericalCoordinates(sdf::ElementPtr _elem);
+
   /// \brief Optional atmosphere model.
   public: std::optional<sdf::Atmosphere> atmosphere;
 
@@ -68,6 +73,10 @@ class sdf::World::Implementation
   public: ignition::math::Vector3d magneticField =
            ignition::math::Vector3d(5.5645e-6, 22.8758e-6, -42.3884e-6);
 
+  /// \brief Spherical coordinates
+  public: std::optional<ignition::math::SphericalCoordinates>
+      sphericalCoordinates;
+
   /// \brief The models specified in this world.
   public: std::vector<Model> models;
 
@@ -179,6 +188,15 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config)
     _sdf->Get<ignition::math::Vector3d>("magnetic_field",
         this->dataPtr->magneticField).first;
 
+  // Read the spherical coordinates
+  if (_sdf->HasElement("spherical_coordinates"))
+  {
+    Errors sphericalCoordsErrors = this->dataPtr->LoadSphericalCoordinates(
+        _sdf->GetElement("spherical_coordinates"));
+    errors.insert(errors.end(), sphericalCoordsErrors.begin(),
+        sphericalCoordsErrors.end());
+  }
+
   if (!_sdf->HasUniqueChildNames())
   {
     sdfwarn << "Non-unique names detected in XML children of world with name["
@@ -405,6 +423,20 @@ void World::SetAtmosphere(const sdf::Atmosphere &_atmosphere)
   this->dataPtr->atmosphere = _atmosphere;
 }
 
+/////////////////////////////////////////////////
+const ignition::math::SphericalCoordinates *
+    World::SphericalCoordinates() const
+{
+  return optionalToPointer(this->dataPtr->sphericalCoordinates);
+}
+
+/////////////////////////////////////////////////
+void World::SetSphericalCoordinates(const ignition::math::SphericalCoordinates
+    &_sphericalCoordinates)
+{
+  this->dataPtr->sphericalCoordinates = _sphericalCoordinates;
+}
+
 /////////////////////////////////////////////////
 const sdf::Gui *World::Gui() const
 {
@@ -647,3 +679,104 @@ void World::SetFrameAttachedToGraph(
     model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
   }
 }
+
+/////////////////////////////////////////////////
+Errors World::Implementation::LoadSphericalCoordinates(
+    sdf::ElementPtr _elem)
+{
+  Errors errors;
+
+  if (_elem->GetName() != "spherical_coordinates")
+  {
+    errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
+        "Attempting to load <spherical_coordinates>, but the provided SDF "
+        "element is a <" + _elem->GetName() + ">."});
+    return errors;
+  }
+
+  // Get elements
+  auto surfaceModel =
+      ignition::math::SphericalCoordinates::SurfaceType::EARTH_WGS84;
+  if (!_elem->HasElement("surface_model"))
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Missing required element <surface_model>"});
+  }
+  else
+  {
+    auto surfaceModelStr = _elem->Get<std::string>("surface_model");
+    if (surfaceModelStr != "EARTH_WGS84")
+    {
+      errors.push_back({ErrorCode::ELEMENT_INVALID,
+          "The supplied <surface_model> [" + surfaceModelStr +
+          "] is not supported."});
+    }
+    surfaceModel = ignition::math::SphericalCoordinates::Convert(
+        surfaceModelStr);
+  }
+
+  std::string worldFrameOrientation{"ENU"};
+  if (_elem->HasElement("world_frame_orientation"))
+  {
+    worldFrameOrientation = _elem->Get<std::string>(
+        "world_frame_orientation");
+    if (worldFrameOrientation != "ENU")
+    {
+      errors.push_back({ErrorCode::ELEMENT_INVALID,
+          "The supplied <world_frame_orientation> [" + worldFrameOrientation +
+          "] is not supported."});
+    }
+  }
+
+  ignition::math::Angle latitude{0.0};
+  if (!_elem->HasElement("latitude_deg"))
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Missing required element <latitude_deg>"});
+  }
+  else
+  {
+    latitude.SetDegree(_elem->Get<double>("latitude_deg"));
+  }
+
+  ignition::math::Angle longitude{0.0};
+  if (!_elem->HasElement("longitude_deg"))
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Missing required element <longitude_deg>"});
+  }
+  else
+  {
+    longitude.SetDegree(_elem->Get<double>("longitude_deg"));
+  }
+
+  double elevation{0.0};
+  if (!_elem->HasElement("elevation"))
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Missing required element <elevation>"});
+  }
+  else
+  {
+    elevation = _elem->Get<double>("elevation");
+  }
+
+  ignition::math::Angle heading{0.0};
+  if (!_elem->HasElement("heading_deg"))
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Missing required element <heading_deg>"});
+  }
+  else
+  {
+    heading.SetDegree(_elem->Get<double>("heading_deg"));
+  }
+
+  // Create coordinates
+  this->sphericalCoordinates.emplace();
+  this->sphericalCoordinates =
+      ignition::math::SphericalCoordinates(surfaceModel, latitude, longitude,
+      elevation, heading);
+
+  return errors;
+}

src/World_TEST.cc

@@ -81,6 +81,7 @@ TEST(DOMWorld, CopyConstructor)
   world.SetAtmosphere(atmosphere);
   world.SetAudioDevice("test_audio_device");
   world.SetGravity({1, 0, 0});
+  world.SetSphericalCoordinates(ignition::math::SphericalCoordinates());
 
   sdf::Gui gui;
   gui.SetFullscreen(true);
@@ -99,6 +100,9 @@ TEST(DOMWorld, CopyConstructor)
 
   ASSERT_TRUE(nullptr != world.Atmosphere());
   EXPECT_DOUBLE_EQ(0.1, world.Atmosphere()->Pressure());
+  ASSERT_TRUE(nullptr != world.SphericalCoordinates());
+  EXPECT_EQ(ignition::math::SphericalCoordinates::EARTH_WGS84,
+      world.SphericalCoordinates()->Surface());
   EXPECT_EQ("test_audio_device", world.AudioDevice());
   EXPECT_EQ(ignition::math::Vector3d::UnitX, world.Gravity());
 

test/integration/world_dom.cc

@@ -107,6 +107,15 @@ TEST(DOMWorld, Load)
   EXPECT_DOUBLE_EQ(4.3, atmosphere->TemperatureGradient());
   EXPECT_DOUBLE_EQ(43.1, atmosphere->Pressure());
 
+  const auto *sphericalCoords = world->SphericalCoordinates();
+  ASSERT_NE(nullptr, sphericalCoords);
+  EXPECT_EQ(ignition::math::SphericalCoordinates::EARTH_WGS84,
+      sphericalCoords->Surface());
+  EXPECT_DOUBLE_EQ(-22.9, sphericalCoords->LatitudeReference().Degree());
+  EXPECT_DOUBLE_EQ(-43.2, sphericalCoords->LongitudeReference().Degree());
+  EXPECT_DOUBLE_EQ(100, sphericalCoords->ElevationReference());
+  EXPECT_DOUBLE_EQ(90, sphericalCoords->HeadingOffset().Degree());
+
   const sdf::Gui *gui = world->Gui();
   ASSERT_NE(nullptr, gui);
   ASSERT_NE(nullptr, gui->Element());

test/sdf/world_complete.sdf

@@ -18,6 +18,14 @@
       <pressure>43.1</pressure>
       <temperature_gradient>4.3</temperature_gradient>
     </atmosphere>
+    <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>100</elevation>
+      <heading_deg>90</heading_deg>
+    </spherical_coordinates>
     <gui fullscreen="true">
     </gui>
     <scene>