diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 00fda70b9..9e620c287 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -18,7 +18,9 @@ #define SDF_WORLD_HH_ #include +#include #include +#include #include #include @@ -137,6 +139,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 diff --git a/sdf/1.9/spherical_coordinates.sdf b/sdf/1.9/spherical_coordinates.sdf index a0aa10354..230be5539 100644 --- a/sdf/1.9/spherical_coordinates.sdf +++ b/sdf/1.9/spherical_coordinates.sdf @@ -12,15 +12,9 @@ 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°. @@ -46,12 +40,10 @@ 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 (from east to north) when viewed from top-down. Note + that this is not consistent with compass heading convention. The angle is specified in degrees. diff --git a/src/World.cc b/src/World.cc index d3041cb49..a6a9d77f9 100644 --- a/src/World.cc +++ b/src/World.cc @@ -39,6 +39,11 @@ using namespace sdf; class sdf::World::Implementation { + /// \brief Populate sphericalCoordinates + /// \param[in] _elem `` element + /// \return Errors, if any. + public: Errors LoadSphericalCoordinates(sdf::ElementPtr _elem); + /// \brief Optional atmosphere model. public: std::optional 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 + sphericalCoordinates; + /// \brief The models specified in this world. public: std::vector models; @@ -179,6 +188,15 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) _sdf->Get("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 , 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 "}); + } + else + { + auto surfaceModelStr = _elem->Get("surface_model"); + if (surfaceModelStr != "EARTH_WGS84") + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "The supplied [" + surfaceModelStr + + "] is not supported."}); + } + surfaceModel = ignition::math::SphericalCoordinates::Convert( + surfaceModelStr); + } + + std::string worldFrameOrientation{"ENU"}; + if (_elem->HasElement("world_frame_orientation")) + { + worldFrameOrientation = _elem->Get( + "world_frame_orientation"); + if (worldFrameOrientation != "ENU") + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "The supplied [" + worldFrameOrientation + + "] is not supported."}); + } + } + + ignition::math::Angle latitude{0.0}; + if (!_elem->HasElement("latitude_deg")) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Missing required element "}); + } + else + { + latitude.SetDegree(_elem->Get("latitude_deg")); + } + + ignition::math::Angle longitude{0.0}; + if (!_elem->HasElement("longitude_deg")) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Missing required element "}); + } + else + { + longitude.SetDegree(_elem->Get("longitude_deg")); + } + + double elevation{0.0}; + if (!_elem->HasElement("elevation")) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Missing required element "}); + } + else + { + elevation = _elem->Get("elevation"); + } + + ignition::math::Angle heading{0.0}; + if (!_elem->HasElement("heading_deg")) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Missing required element "}); + } + else + { + heading.SetDegree(_elem->Get("heading_deg")); + } + + // Create coordinates + this->sphericalCoordinates.emplace(); + this->sphericalCoordinates = + ignition::math::SphericalCoordinates(surfaceModel, latitude, longitude, + elevation, heading); + + return errors; +} diff --git a/src/World_TEST.cc b/src/World_TEST.cc index f48e0eca2..c2d67012c 100644 --- a/src/World_TEST.cc +++ b/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()); diff --git a/test/integration/world_dom.cc b/test/integration/world_dom.cc index 1e1aa1ab7..9ad37bf00 100644 --- a/test/integration/world_dom.cc +++ b/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()); diff --git a/test/sdf/world_complete.sdf b/test/sdf/world_complete.sdf index 57957c1ef..5c38f46a2 100644 --- a/test/sdf/world_complete.sdf +++ b/test/sdf/world_complete.sdf @@ -18,6 +18,14 @@ 43.1 4.3 + + EARTH_WGS84 + ENU + -22.9 + -43.2 + 100 + 90 +