From adf26442c9d4adc9074c68afd6aa68a4393a3a8f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 15 Sep 2021 17:32:57 -0700 Subject: [PATCH] =?UTF-8?q?=F0=9F=8C=90=20Spherical=20coordinates=20conver?= =?UTF-8?q?sion=20and=20commands=20(#177)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- include/ignition/msgs/Utility.hh | 25 ++++++++++ proto/ignition/msgs/entity_factory.proto | 11 ++++- .../ignition/msgs/spherical_coordinates.proto | 20 +++++++- src/Utility.cc | 49 +++++++++++++++++++ src/Utility_TEST.cc | 38 ++++++++++++++ 5 files changed, 139 insertions(+), 4 deletions(-) diff --git a/include/ignition/msgs/Utility.hh b/include/ignition/msgs/Utility.hh index 04ca4c65..e353920e 100644 --- a/include/ignition/msgs/Utility.hh +++ b/include/ignition/msgs/Utility.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include "ignition/msgs/config.hh" @@ -85,6 +86,14 @@ namespace ignition IGNITION_MSGS_VISIBLE math::Inertiald Convert(const msgs::Inertial &_i); + /// \brief Convert a msgs::SphericalCoordinates to an + /// ignition::math::SphericalCoordinates + /// \param[in] _coord The spherical coordinates to convert + /// \return An ignition::math::SphericalCoordinates object + IGNITION_MSGS_VISIBLE + math::SphericalCoordinates Convert( + const msgs::SphericalCoordinates &_coord); + /// \brief Convert a msgs::AxisAlignedBox to an /// ignition::math::AxisAlignedBox /// \param[in] _b The axis aligned box to convert @@ -195,6 +204,14 @@ namespace ignition IGNITION_MSGS_VISIBLE msgs::Inertial Convert(const math::MassMatrix3d &_m); + /// \brief Convert an math::SphericalCoordinates to a + /// msgs::SphericalCoordinates + /// \param[in] _coord The SphericalCoordinates to convert + /// \return A msgs::SphericalCoordinates object + IGNITION_MSGS_VISIBLE + msgs::SphericalCoordinates Convert( + const math::SphericalCoordinates &_coord); + /// \brief Convert a ignition::math::Planed to a msgs::PlaneGeom /// \param[in] _p The plane to convert /// \return A msgs::PlaneGeom object @@ -352,6 +369,14 @@ namespace ignition IGNITION_MSGS_VISIBLE void Set(msgs::Inertial *_i, const math::MassMatrix3d &_m); + /// \brief Set a msgs::SphericalCoordinates from a + /// math::SphericalCoordinates + /// \param[out] _sc A msgs::SphericalCoordinates pointer + /// \param[in] _m An math::SphericalCoordinates reference + IGNITION_MSGS_VISIBLE + void Set(msgs::SphericalCoordinates *_sc, + const math::SphericalCoordinates &_m); + /// \brief Set a msgs::Plane from an ignition::math::Planed /// \param[out] _p A msgs::Plane pointer /// \param[in] _v An ignition::math::Planed reference diff --git a/proto/ignition/msgs/entity_factory.proto b/proto/ignition/msgs/entity_factory.proto index 246228f2..29a8e0a7 100644 --- a/proto/ignition/msgs/entity_factory.proto +++ b/proto/ignition/msgs/entity_factory.proto @@ -30,10 +30,11 @@ option java_outer_classname = "EntityFactoryProtos"; /// 3. From a message (model / light) /// 4. Cloning an existing entity (clone_name) -import "ignition/msgs/pose.proto"; +import "ignition/msgs/header.proto"; import "ignition/msgs/light.proto"; import "ignition/msgs/model.proto"; -import "ignition/msgs/header.proto"; +import "ignition/msgs/pose.proto"; +import "ignition/msgs/spherical_coordinates.proto"; message EntityFactory { @@ -60,6 +61,7 @@ message EntityFactory } /// \brief Pose where the entity will be spawned in the world. + /// If set, `spherical_coordinates` will be ignored. Pose pose = 7; /// \brief New name for the entity, overrides the name on the SDF. @@ -72,4 +74,9 @@ message EntityFactory /// \brief The pose will be defined relative to this frame. If left empty, /// the "world" frame will be used. string relative_to = 10; + + /// \brief Spherical coordinates where the entity will be spawned in the + /// world. + /// It's ignored if `pose` is set. + SphericalCoordinates spherical_coordinates = 11; } diff --git a/proto/ignition/msgs/spherical_coordinates.proto b/proto/ignition/msgs/spherical_coordinates.proto index 016c90e0..15309b64 100644 --- a/proto/ignition/msgs/spherical_coordinates.proto +++ b/proto/ignition/msgs/spherical_coordinates.proto @@ -22,23 +22,39 @@ option java_outer_classname = "SphericalCoordinatesProtos"; /// \ingroup ignition.msgs /// \interface SphericalCoordinates -/// \brief Spherical coordinates information +/// \brief Spherical coordinates information +import "ignition/msgs/entity.proto"; import "ignition/msgs/header.proto"; message SphericalCoordinates { + /// \brief Planetary surface models. enum SurfaceModel { + /// \brief World Geodetic System 1984 EARTH_WGS84 = 0; } - /// \brief Optional header data + /// \brief Optional header data. Header header = 1; + /// \brief Planetary surface model. SurfaceModel surface_model = 2; + + /// \brief Latitude in degrees. double latitude_deg = 3; + + /// \brief Longitude in degrees. double longitude_deg = 4; + + /// \brief Elevation in meters. double elevation = 5; + + /// \brief Heading in degrees. double heading_deg = 6; + + /// \brief Entity that the coordinates apply to. + /// If not set, defaults to the world origin. + Entity entity = 7; } diff --git a/src/Utility.cc b/src/Utility.cc index 217dc36d..6c312a42 100644 --- a/src/Utility.cc +++ b/src/Utility.cc @@ -120,6 +120,27 @@ namespace ignition pose); } + ///////////////////////////////////////////// + math::SphericalCoordinates Convert(const msgs::SphericalCoordinates &_sc) + { + math::SphericalCoordinates out; + if (_sc.surface_model() == msgs::SphericalCoordinates::EARTH_WGS84) + { + out.SetSurface(math::SphericalCoordinates::EARTH_WGS84); + } + else + { + std::cerr << "Unrecognized spherical surface type [" + << _sc.surface_model() + << "]. Using default." << std::endl; + } + out.SetLatitudeReference(IGN_DTOR(_sc.latitude_deg())); + out.SetLongitudeReference(IGN_DTOR(_sc.longitude_deg())); + out.SetElevationReference(_sc.elevation()); + out.SetHeadingOffset(IGN_DTOR(_sc.heading_deg())); + return out; + } + ///////////////////////////////////////////// math::AxisAlignedBox Convert(const msgs::AxisAlignedBox &_b) { @@ -278,6 +299,14 @@ namespace ignition return result; } + ///////////////////////////////////////////// + msgs::SphericalCoordinates Convert(const math::SphericalCoordinates &_sc) + { + msgs::SphericalCoordinates result; + msgs::Set(&result, _sc); + return result; + } + ///////////////////////////////////////////// msgs::PlaneGeom Convert(const ignition::math::Planed &_p) { @@ -433,6 +462,26 @@ namespace ignition msgs::Set(_i->mutable_pose(), _m.Pose()); } + ///////////////////////////////////////////////// + void Set(msgs::SphericalCoordinates *_sc, + const math::SphericalCoordinates &_m) + { + if (_m.Surface() == math::SphericalCoordinates::EARTH_WGS84) + { + _sc->set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84); + } + else + { + std::cerr << "Unrecognized spherical surface type [" + << _m.Surface() + << "]. Not populating message field." << std::endl; + } + _sc->set_latitude_deg(_m.LatitudeReference().Degree()); + _sc->set_longitude_deg(_m.LongitudeReference().Degree()); + _sc->set_elevation(_m.ElevationReference()); + _sc->set_heading_deg(_m.HeadingOffset().Degree()); + } + ///////////////////////////////////////////////// void Set(msgs::StringMsg *_p, const std::string &_v) { diff --git a/src/Utility_TEST.cc b/src/Utility_TEST.cc index e98614f4..caa4ea54 100644 --- a/src/Utility_TEST.cc +++ b/src/Utility_TEST.cc @@ -236,6 +236,29 @@ TEST(MsgsTest, ConvertMathMassMatrix3ToMsgs) EXPECT_EQ(ignition::math::Pose3d::Zero, msgs::Convert(msg.pose())); } +///////////////////////////////////////////////// +TEST(MsgsTest, ConvertMathSphericalCoordinatesToMsgs) +{ + auto msg = msgs::Convert( + math::SphericalCoordinates( + math::SphericalCoordinates::SurfaceType::EARTH_WGS84, + IGN_DTOR(1.1), IGN_DTOR(2.2), 3.3, IGN_DTOR(0.4))); + + EXPECT_EQ(msgs::SphericalCoordinates::EARTH_WGS84, msg.surface_model()); + EXPECT_DOUBLE_EQ(1.1, msg.latitude_deg()); + EXPECT_DOUBLE_EQ(2.2, msg.longitude_deg()); + EXPECT_DOUBLE_EQ(3.3, msg.elevation()); + EXPECT_DOUBLE_EQ(0.4, msg.heading_deg()); + + auto math = msgs::Convert(msg); + + EXPECT_EQ(math::SphericalCoordinates::EARTH_WGS84, math.Surface()); + EXPECT_DOUBLE_EQ(1.1, math.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(2.2, math.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(3.3, math.ElevationReference()); + EXPECT_DOUBLE_EQ(0.4, math.HeadingOffset().Degree()); +} + ///////////////////////////////////////////////// TEST(UtilityTest, ConvertStringMsg) { @@ -451,6 +474,21 @@ TEST(MsgsTest, SetMassMatrix3) EXPECT_EQ(ignition::math::Pose3d::Zero, msgs::Convert(msg.pose())); } +///////////////////////////////////////////////// +TEST(MsgsTest, SetSphericalCoordinates) +{ + msgs::SphericalCoordinates msg; + msgs::Set(&msg, math::SphericalCoordinates( + math::SphericalCoordinates::SurfaceType::EARTH_WGS84, + IGN_DTOR(1.1), IGN_DTOR(2.2), 3.3, IGN_DTOR(0.4))); + + EXPECT_EQ(msgs::SphericalCoordinates::EARTH_WGS84, msg.surface_model()); + EXPECT_DOUBLE_EQ(1.1, msg.latitude_deg()); + EXPECT_DOUBLE_EQ(2.2, msg.longitude_deg()); + EXPECT_DOUBLE_EQ(3.3, msg.elevation()); + EXPECT_DOUBLE_EQ(0.4, msg.heading_deg()); +} + ///////////////////////////////////////////////// TEST(MsgsTest, SetStringMsg) {