🌐 Spherical coordinates conversion and commands (#177)

Signed-off-by: Louise Poubel <[email protected]>

include/ignition/msgs/Utility.hh

@@ -26,6 +26,7 @@
 #include <ignition/math/Inertial.hh>
 #include <ignition/math/Pose3.hh>
 #include <ignition/math/Plane.hh>
+#include <ignition/math/SphericalCoordinates.hh>
 #include <ignition/math/Vector3.hh>
 
 #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

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;
 }

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;
 }

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)
     {

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)
 {