🌐 Spherical coordinates (#1008)

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

CMakeLists.txt

@@ -134,7 +134,7 @@ set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-math
-ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.8)
+ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.9)
 set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR})
 
 #--------------------------------------

examples/worlds/spherical_coordinates.sdf

@@ -0,0 +1,106 @@
+<?xml version="1.0" ?>
+<!--
+  Latitude / longitude demo
+
+  The world origin's spherical coordinates can be inspected using the Component
+  Inspector GUI plugin.
+
+  Try changing the world origin coordinates with:
+
+ign service \
+-s /world/spherical_coordinates/set_spherical_coordinates \
+--reqtype ignition.msgs.SphericalCoordinates \
+--reptype ignition.msgs.Boolean \
+--timeout 2000 \
+--req "surface_model: EARTH_WGS84, latitude_deg: 35.6, longitude_deg: 140.1, elevation: 10.0"
+
+  Spawn a model in spherical coordinates:
+
+ign service -s /world/spherical_coordinates/create \
+--reqtype ignition.msgs.EntityFactory \
+--reptype ignition.msgs.Boolean \
+--timeout 300 \
+--req 'sdf: '\
+'"<?xml version=\"1.0\" ?>'\
+'<sdf version=\"1.6\">'\
+'<model name=\"spawned_model\">'\
+'<link name=\"link\">'\
+'<visual name=\"visual\">'\
+'<geometry><sphere><radius>1.0</radius></sphere></geometry>'\
+'</visual>'\
+'<collision name=\"visual\">'\
+'<geometry><sphere><radius>1.0</radius></sphere></geometry>'\
+'</collision>'\
+'</link>'\
+'</model>'\
+'</sdf>" '\
+'spherical_coordinates: {latitude_deg: 35.59999, longitude_deg: 140.09999, elevation: 11.0} '
+
+  Move a model in spherical coordinates:
+
+ign service -s /world/spherical_coordinates/set_spherical_coordinates \
+--reqtype ignition.msgs.SphericalCoordinates \
+--reptype ignition.msgs.Boolean \
+--timeout 300 \
+--req 'entity: {name: "spawned_model", type: MODEL}, latitude_deg: 35.59990, longitude_deg: 140.09990'
+
+-->
+<sdf version="1.6">
+  <world name="spherical coordinates">
+
+    <!-- Handles requests to update the spherical coordinates -->
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+
+    <!-- Populates the GUI with data from the server -->
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <!-- Processes pose commands -->
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+
+    <!-- Set the coordinates for the world origin -->
+    <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>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+
+    <gui fullscreen="0">
+
+      <!-- 3D scene -->
+      <plugin filename="GzScene3D" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>0.4 0.4 0.4</ambient_light>
+        <background_color>0.8 0.8 0.8</background_color>
+        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+      </plugin>
+
+      <!-- Component inspector -->
+      <plugin filename="ComponentInspector" name="Component Inspector">
+        <ignition-gui>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+      </plugin>
+
+    </gui>
+
+  </world>
+</sdf>

include/ignition/gazebo/Util.hh

@@ -238,6 +238,15 @@ namespace ignition
       return changed;
     }
 
+    /// \brief Get the spherical coordinates for an entity.
+    /// \param[in] _entity Entity whose coordinates we want.
+    /// \param[in] _ecm Entity component manager
+    /// \return The entity's latitude (deg), longitude (deg) and elevation (m).
+    /// If the entity doesn't have a pose, or the world's spherical coordinates
+    /// haven't been defined, this will return nullopt.
+    std::optional<math::Vector3d> IGNITION_GAZEBO_VISIBLE sphericalCoordinates(
+        Entity _entity, const EntityComponentManager &_ecm);
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"};
 

include/ignition/gazebo/World.hh

@@ -24,6 +24,7 @@
 
 #include <sdf/Atmosphere.hh>
 #include <ignition/math/Vector3.hh>
+#include <ignition/math/SphericalCoordinates.hh>
 
 #include "ignition/gazebo/config.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
@@ -116,11 +117,24 @@ namespace ignition
 
       /// \brief Get atmosphere information.
       /// \param[in] _ecm Entity-component manager.
-      /// \return Magnetic field vector or nullopt if the entity does not
+      /// \return Atmosphere or nullopt if the entity does not
       /// have a components::Atmosphere component.
       public: std::optional<sdf::Atmosphere> Atmosphere(
           const EntityComponentManager &_ecm) const;
 
+      /// \brief Get spherical coordinates for the world origin.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Spherical coordinates or nullopt if the entity does not
+      /// have a components::SphericalCoordinates component.
+      public: std::optional<math::SphericalCoordinates> SphericalCoordinates(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Set spherical coordinates for the world origin.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _sphericalCoordinates New spherical coordinates.
+      public: void SetSphericalCoordinates(EntityComponentManager &_ecm,
+          const math::SphericalCoordinates &_sphericalCoordinates);
+
       /// \brief Get the ID of a light entity which is an immediate child of
       /// this world.
       /// \param[in] _ecm Entity-component manager.

include/ignition/gazebo/components/Serialization.hh

@@ -25,6 +25,7 @@
 #include <sdf/Sensor.hh>
 
 #include <ignition/gazebo/Conversions.hh>
+#include <ignition/msgs/Utility.hh>
 
 // This header holds serialization operators which are shared among several
 // components
@@ -35,6 +36,31 @@ namespace gazebo
 {
 // Inline bracket to help doxygen filtering.
 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace traits
+{
+  /// \brief Type trait that determines if an ignition::gazebo::convert from In
+  /// to Out is defined.
+  /// Usage:
+  /// \code
+  ///    constexpr bool hasGazeboConvert =
+  ///       HasGazeboConvert<math::Pose, msgs::Pose>::value
+  /// \endcode
+  template <typename In, typename Out>
+  class HasGazeboConvert
+  {
+    private: template <typename InArg, typename OutArg>
+    static auto Test(int _test)
+        -> decltype(std::declval<OutArg>() =
+           ignition::gazebo::convert<OutArg>(std::declval<const InArg &>()),
+           std::true_type());
+
+    private: template <typename, typename>
+    static auto Test(...) -> std::false_type;
+
+    public: static constexpr bool value =  // NOLINT
+                decltype(Test<In, Out>(true))::value;
+  };
+}
 /// \brief A Serializer class is used to serialize and deserialize a component.
 /// It is passed in as the third template parameter to components::Component.
 /// Eg.
@@ -78,7 +104,16 @@ namespace serializers
     public: static std::ostream &Serialize(std::ostream &_out,
                                            const DataType &_data)
     {
-      auto msg = ignition::gazebo::convert<MsgType>(_data);
+      MsgType msg;
+      if constexpr (traits::HasGazeboConvert<DataType, MsgType>::value)
+      {
+        msg = ignition::gazebo::convert<MsgType>(_data);
+      }
+      else
+      {
+        msg = ignition::msgs::Convert(_data);
+      }
+
       msg.SerializeToOstream(&_out);
       return _out;
     }
@@ -93,7 +128,14 @@ namespace serializers
       MsgType msg;
       msg.ParseFromIstream(&_in);
 
-      _data = ignition::gazebo::convert<DataType>(msg);
+      if constexpr (traits::HasGazeboConvert<MsgType, DataType>::value)
+      {
+        _data = ignition::gazebo::convert<DataType>(msg);
+      }
+      else
+      {
+        _data = ignition::msgs::Convert(msg);
+      }
       return _in;
     }
   };

include/ignition/gazebo/components/SphericalCoordinates.hh

@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_COMPONENTS_SPHERICALCOORDINATES_HH_
+#define IGNITION_GAZEBO_COMPONENTS_SPHERICALCOORDINATES_HH_
+
+#include <ignition/math/SphericalCoordinates.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/Conversions.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace serializers
+{
+  using SphericalCoordinatesSerializer =
+      serializers::ComponentToMsgSerializer<math::SphericalCoordinates,
+      msgs::SphericalCoordinates>;
+}
+
+namespace components
+{
+  /// \brief This component holds the spherical coordinates of the world origin.
+  using SphericalCoordinates =
+      Component<math::SphericalCoordinates, class SphericalCoordinatesTag,
+      serializers::SphericalCoordinatesSerializer>;
+  IGN_GAZEBO_REGISTER_COMPONENT(
+      "ign_gazebo_components.SphericalCoordinates", SphericalCoordinates)
+}
+}
+}
+}
+
+#endif

src/LevelManager.cc

@@ -25,6 +25,7 @@
 #include <sdf/Model.hh>
 #include <sdf/World.hh>
 
+#include <ignition/math/SphericalCoordinates.hh>
 #include <ignition/common/Profiler.hh>
 
 #include "ignition/gazebo/Events.hh"
@@ -53,6 +54,7 @@
 #include "ignition/gazebo/components/RenderEngineServerHeadless.hh"
 #include "ignition/gazebo/components/RenderEngineServerPlugin.hh"
 #include "ignition/gazebo/components/Scene.hh"
+#include "ignition/gazebo/components/SphericalCoordinates.hh"
 #include "ignition/gazebo/components/Wind.hh"
 #include "ignition/gazebo/components/World.hh"
 
@@ -185,6 +187,14 @@ void LevelManager::ReadLevelPerformerInfo()
         components::Atmosphere(*this->runner->sdfWorld->Atmosphere()));
   }
 
+  // spherical coordinates
+  if (this->runner->sdfWorld->SphericalCoordinates())
+  {
+    this->runner->entityCompMgr.CreateComponent(this->worldEntity,
+        components::SphericalCoordinates(
+        *this->runner->sdfWorld->SphericalCoordinates()));
+  }
+
   // TODO(anyone) This should probably go somewhere else as it is a global
   // constant.
   const std::string kPluginName{"ignition::gazebo"};

src/SdfEntityCreator.cc

@@ -66,6 +66,7 @@
 #include "ignition/gazebo/components/SelfCollide.hh"
 #include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/SourceFilePath.hh"
+#include "ignition/gazebo/components/SphericalCoordinates.hh"
 #include "ignition/gazebo/components/Static.hh"
 #include "ignition/gazebo/components/ThermalCamera.hh"
 #include "ignition/gazebo/components/ThreadPitch.hh"
@@ -237,6 +238,13 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world)
         components::Atmosphere(*_world->Atmosphere()));
   }
 
+  // spherical coordinates
+  if (_world->SphericalCoordinates())
+  {
+    this->dataPtr->ecm->CreateComponent(worldEntity,
+        components::SphericalCoordinates(*_world->SphericalCoordinates()));
+  }
+
   // Models
   for (uint64_t modelIndex = 0; modelIndex < _world->ModelCount();
       ++modelIndex)

src/Util.cc

@@ -47,6 +47,7 @@
 #include "ignition/gazebo/components/ParticleEmitter.hh"
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/Sensor.hh"
+#include "ignition/gazebo/components/SphericalCoordinates.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/World.hh"
 
@@ -554,6 +555,30 @@ std::string validTopic(const std::vector<std::string> &_topics)
   }
   return std::string();
 }
+
+//////////////////////////////////////////////////
+std::optional<math::Vector3d> sphericalCoordinates(Entity _entity,
+    const EntityComponentManager &_ecm)
+{
+  auto sphericalCoordinatesComp =
+      _ecm.Component<components::SphericalCoordinates>(
+      worldEntity(_entity, _ecm));
+  if (nullptr == sphericalCoordinatesComp)
+  {
+    return std::nullopt;
+  }
+
+  auto xyzPose = worldPose(_entity, _ecm);
+
+  // lat / lon / elevation in rad / rad / m
+  auto rad = sphericalCoordinatesComp->Data().PositionTransform(
+      xyzPose.Pos(),
+      math::SphericalCoordinates::LOCAL2,
+      math::SphericalCoordinates::SPHERICAL);
+
+  // Return degrees
+  return math::Vector3d(IGN_RTOD(rad.X()), IGN_RTOD(rad.Y()), rad.Z());
+}
 }
 }
 }