diff --git a/proto/gz/msgs/spherical_coordinates.proto b/proto/gz/msgs/spherical_coordinates.proto index 8511d8ea..9b485277 100644 --- a/proto/gz/msgs/spherical_coordinates.proto +++ b/proto/gz/msgs/spherical_coordinates.proto @@ -33,28 +33,42 @@ message SphericalCoordinates enum SurfaceModel { /// \brief World Geodetic System 1984 - EARTH_WGS84 = 0; + EARTH_WGS84 = 0; + + /// \brief Model of the moon, based on the Selenographic + /// coordinate system, see wikipedia: Selenographic + /// Coordinate System. + MOON_SCS = 1; + + /// \brief Custom surface type + CUSTOM_SURFACE = 2; } /// \brief Optional header data. - Header header = 1; + Header header = 1; /// \brief Planetary surface model. - SurfaceModel surface_model = 2; + SurfaceModel surface_model = 2; /// \brief Latitude in degrees. - double latitude_deg = 3; + double latitude_deg = 3; /// \brief Longitude in degrees. - double longitude_deg = 4; + double longitude_deg = 4; /// \brief Elevation in meters. - double elevation = 5; + double elevation = 5; /// \brief Heading in degrees. - double heading_deg = 6; + double heading_deg = 6; /// \brief Entity that the coordinates apply to. /// If not set, defaults to the world origin. - Entity entity = 7; + Entity entity = 7; + + /// \brief Equatorial axis in meters. + double surface_axis_equatorial = 8; + + /// \brief Polar axis in meters. + double surface_axis_polar = 9; } diff --git a/src/Utility.cc b/src/Utility.cc index 491b4e38..ee8ee541 100644 --- a/src/Utility.cc +++ b/src/Utility.cc @@ -130,6 +130,16 @@ namespace gz { out.SetSurface(math::SphericalCoordinates::EARTH_WGS84); } + else if (_sc.surface_model() == msgs::SphericalCoordinates::MOON_SCS) + { + out.SetSurface(math::SphericalCoordinates::MOON_SCS); + } + else if (_sc.surface_model() == + msgs::SphericalCoordinates::CUSTOM_SURFACE) + { + out.SetSurface(math::SphericalCoordinates::CUSTOM_SURFACE, + _sc.surface_axis_equatorial(), _sc.surface_axis_polar()); + } else { std::cerr << "Unrecognized spherical surface type [" @@ -472,6 +482,18 @@ namespace gz { _sc->set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84); } + else if (_m.Surface() == math::SphericalCoordinates::MOON_SCS) + { + _sc->set_surface_model(msgs::SphericalCoordinates::MOON_SCS); + } + else if (_m.Surface() == + math::SphericalCoordinates::CUSTOM_SURFACE) + { + _sc->set_surface_model( + msgs::SphericalCoordinates::CUSTOM_SURFACE); + _sc->set_surface_axis_equatorial(_m.SurfaceAxisEquatorial()); + _sc->set_surface_axis_polar(_m.SurfaceAxisPolar()); + } else { std::cerr << "Unrecognized spherical surface type [" diff --git a/src/Utility_TEST.cc b/src/Utility_TEST.cc index 78182555..1d22d9a3 100644 --- a/src/Utility_TEST.cc +++ b/src/Utility_TEST.cc @@ -258,6 +258,59 @@ TEST(MsgsTest, ConvertMathSphericalCoordinatesToMsgs) EXPECT_DOUBLE_EQ(2.2, math.LongitudeReference().Degree()); EXPECT_DOUBLE_EQ(3.3, math.ElevationReference()); EXPECT_DOUBLE_EQ(0.4, math.HeadingOffset().Degree()); + + // For Moon's surface. + auto msgMoon = msgs::Convert( + math::SphericalCoordinates( + math::SphericalCoordinates::SurfaceType::MOON_SCS, + GZ_DTOR(1.1), GZ_DTOR(2.2), 3.3, GZ_DTOR(0.4))); + + EXPECT_EQ(msgs::SphericalCoordinates::MOON_SCS, + msgMoon.surface_model()); + EXPECT_DOUBLE_EQ(1.1, msgMoon.latitude_deg()); + EXPECT_DOUBLE_EQ(2.2, msgMoon.longitude_deg()); + EXPECT_DOUBLE_EQ(3.3, msgMoon.elevation()); + EXPECT_DOUBLE_EQ(0.4, msgMoon.heading_deg()); + + auto mathMoon = msgs::Convert(msgMoon); + + EXPECT_EQ(math::SphericalCoordinates::MOON_SCS, + mathMoon.Surface()); + EXPECT_DOUBLE_EQ(1.1, mathMoon.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(2.2, mathMoon.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(3.3, mathMoon.ElevationReference()); + EXPECT_DOUBLE_EQ(0.4, mathMoon.HeadingOffset().Degree()); + + // For custom surfaces. + auto sc = math::SphericalCoordinates( + math::SphericalCoordinates::CUSTOM_SURFACE, + 12000, 10000); + sc.SetLatitudeReference(GZ_DTOR(1.1)); + sc.SetLongitudeReference(GZ_DTOR(2.2)); + sc.SetElevationReference(3.3); + sc.SetHeadingOffset(GZ_DTOR(0.4)); + + auto msgCustom = msgs::Convert(sc); + + EXPECT_EQ(msgs::SphericalCoordinates::CUSTOM_SURFACE, + msgCustom.surface_model()); + EXPECT_DOUBLE_EQ(1.1, msgCustom.latitude_deg()); + EXPECT_DOUBLE_EQ(2.2, msgCustom.longitude_deg()); + EXPECT_DOUBLE_EQ(3.3, msgCustom.elevation()); + EXPECT_DOUBLE_EQ(0.4, msgCustom.heading_deg()); + EXPECT_DOUBLE_EQ(12000, msgCustom.surface_axis_equatorial()); + EXPECT_DOUBLE_EQ(10000, msgCustom.surface_axis_polar()); + + auto mathCustom = msgs::Convert(msgCustom); + + EXPECT_EQ(math::SphericalCoordinates::CUSTOM_SURFACE, + mathCustom.Surface()); + EXPECT_DOUBLE_EQ(1.1, mathCustom.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(2.2, mathCustom.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(3.3, mathCustom.ElevationReference()); + EXPECT_DOUBLE_EQ(0.4, mathCustom.HeadingOffset().Degree()); + EXPECT_DOUBLE_EQ(12000, mathCustom.SurfaceAxisEquatorial()); + EXPECT_DOUBLE_EQ(10000, mathCustom.SurfaceAxisPolar()); } ///////////////////////////////////////////////// @@ -488,6 +541,40 @@ TEST(MsgsTest, SetSphericalCoordinates) EXPECT_DOUBLE_EQ(2.2, msg.longitude_deg()); EXPECT_DOUBLE_EQ(3.3, msg.elevation()); EXPECT_DOUBLE_EQ(0.4, msg.heading_deg()); + + // For Moon's surface. + msgs::SphericalCoordinates msgMoon; + msgs::Set(&msgMoon, math::SphericalCoordinates( + math::SphericalCoordinates::SurfaceType::MOON_SCS, + GZ_DTOR(1.2), GZ_DTOR(2.3), 3.4, GZ_DTOR(0.5))); + + EXPECT_EQ(msgs::SphericalCoordinates::MOON_SCS, + msgMoon.surface_model()); + EXPECT_DOUBLE_EQ(1.2, msgMoon.latitude_deg()); + EXPECT_DOUBLE_EQ(2.3, msgMoon.longitude_deg()); + EXPECT_DOUBLE_EQ(3.4, msgMoon.elevation()); + EXPECT_DOUBLE_EQ(0.5, msgMoon.heading_deg()); + + // For a custom surface. + msgs::SphericalCoordinates msgCustom; + auto sc = math::SphericalCoordinates( + math::SphericalCoordinates::CUSTOM_SURFACE, + 12000, 10000); + sc.SetLatitudeReference(GZ_DTOR(1.9)); + sc.SetLongitudeReference(GZ_DTOR(2.8)); + sc.SetElevationReference(3.7); + sc.SetHeadingOffset(GZ_DTOR(0.6)); + + msgs::Set(&msgCustom, sc); + + EXPECT_EQ(msgs::SphericalCoordinates::CUSTOM_SURFACE, + msgCustom.surface_model()); + EXPECT_DOUBLE_EQ(1.9, msgCustom.latitude_deg()); + EXPECT_DOUBLE_EQ(2.8, msgCustom.longitude_deg()); + EXPECT_DOUBLE_EQ(3.7, msgCustom.elevation()); + EXPECT_DOUBLE_EQ(0.6, msgCustom.heading_deg()); + EXPECT_DOUBLE_EQ(12000, msgCustom.surface_axis_equatorial()); + EXPECT_DOUBLE_EQ(10000, msgCustom.surface_axis_polar()); } /////////////////////////////////////////////////